Skip to content

Commit

Permalink
fmu:Add MODE_4PWM1CAP, MODE_5PWM, MODE_5PWM1CAP
Browse files Browse the repository at this point in the history 
   This extends the  Capture support for FMU
   CHAN 5 and 6.
  • Loading branch information
David Sidrane committed Sep 25, 2018
1 parent 2b3b0eb commit 60d0a08
Show file tree
Hide file tree
Showing 2 changed files with 113 additions and 8 deletions.
17 changes: 11 additions & 6 deletions src/drivers/drv_pwm_output.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,12 +259,17 @@ struct pwm_output_rc_config {
#define PWM_SERVO_MODE_3PWM 4
#define PWM_SERVO_MODE_3PWM1CAP 5
#define PWM_SERVO_MODE_4PWM 6
#define PWM_SERVO_MODE_6PWM 7
#define PWM_SERVO_MODE_8PWM 8
#define PWM_SERVO_MODE_14PWM 9
#define PWM_SERVO_MODE_4CAP 10
#define PWM_SERVO_MODE_5CAP 11
#define PWM_SERVO_MODE_6CAP 12
#define PWM_SERVO_MODE_4PWM1CAP 7
#define PWM_SERVO_MODE_5PWM 8
#define PWM_SERVO_MODE_5PWM1CAP 9
#define PWM_SERVO_MODE_6PWM 10
#define PWM_SERVO_MODE_8PWM 11
#define PWM_SERVO_MODE_14PWM 12
#define PWM_SERVO_MODE_4CAP 13
#define PWM_SERVO_MODE_5CAP 14
#define PWM_SERVO_MODE_6CAP 15
#define PWM_SERVO_ENTER_TEST_MODE 16
#define PWM_SERVO_EXIT_TEST_MODE 17
#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34)

/*
Expand Down
104 changes: 102 additions & 2 deletions src/drivers/px4fmu/fmu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,11 +83,14 @@ enum PortMode {
PORT_FULL_GPIO,
PORT_FULL_PWM,
PORT_PWM6,
PORT_PWM5,
PORT_PWM4,
PORT_PWM3,
PORT_PWM2,
PORT_PWM1,
PORT_PWM3CAP1,
PORT_PWM4CAP1,
PORT_PWM5CAP1,
PORT_PWM2CAP2,
PORT_CAPTURE,
};
Expand All @@ -107,6 +110,9 @@ class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
MODE_3PWM,
MODE_3PWM1CAP,
MODE_4PWM,
MODE_4PWM1CAP,
MODE_5PWM,
MODE_5PWM1CAP,
MODE_6PWM,
MODE_8PWM,
MODE_14PWM,
Expand Down Expand Up @@ -601,6 +607,15 @@ PX4FMU::set_mode(Mode mode)

break;

#if defined(BOARD_HAS_CAPTURE)

case MODE_4PWM1CAP:
PX4_DEBUG("MODE_4PWM1CAP");
up_input_capture_set(4, Rising, 0, NULL, NULL);
#endif

/* FALLTHROUGH */

case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
DEVICE_DEBUG("MODE_4PWM");

Expand All @@ -614,6 +629,28 @@ PX4FMU::set_mode(Mode mode)

break;

#if defined(BOARD_HAS_CAPTURE)

case MODE_5PWM1CAP:
PX4_DEBUG("MODE_5PWM1CAP");
up_input_capture_set(5, Rising, 0, NULL, NULL);
#endif

/* FALLTHROUGH */

case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs
PX4_DEBUG("MODE_5PWM");

/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_mask = 0x1f;
_pwm_initialized = false;
_num_outputs = 4;

break;

#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6

case MODE_6PWM:
Expand Down Expand Up @@ -1446,8 +1483,11 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
case MODE_2PWM:
case MODE_3PWM:
case MODE_4PWM:
case MODE_5PWM:
case MODE_2PWM2CAP:
case MODE_3PWM1CAP:
case MODE_4PWM1CAP:
case MODE_5PWM1CAP:
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
case MODE_6PWM:
#endif
Expand Down Expand Up @@ -1923,7 +1963,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
#endif

case MODE_5PWM:
case MODE_5PWM1CAP:
*(unsigned *)arg = 5;
break;

case MODE_4PWM:
case MODE_4PWM1CAP:
*(unsigned *)arg = 4;
break;

Expand Down Expand Up @@ -1975,6 +2021,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_4PWM);
break;

case 5:
set_mode(MODE_5PWM);
break;

#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6

case 6:
Expand Down Expand Up @@ -2027,6 +2077,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = set_mode(MODE_4PWM);
break;

case PWM_SERVO_MODE_4PWM1CAP:
ret = set_mode(MODE_4PWM1CAP);
break;

case PWM_SERVO_MODE_5PWM:
ret = set_mode(MODE_5PWM);
break;

case PWM_SERVO_MODE_5PWM1CAP:
ret = set_mode(MODE_5PWM1CAP);
break;

case PWM_SERVO_MODE_6PWM:
ret = set_mode(MODE_6PWM);
break;
Expand Down Expand Up @@ -2325,7 +2387,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)

input_capture_stats_t *stats = (input_capture_stats_t *)arg;

if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP) {
if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP ||
_mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP) {

pconfig = (input_capture_config_t *)arg;
}
Expand Down Expand Up @@ -2400,6 +2463,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
ret = OK;

switch (_mode) {
case MODE_5PWM1CAP:
case MODE_4PWM1CAP:
case MODE_3PWM1CAP:
*(unsigned *)arg = 1;
break;
Expand Down Expand Up @@ -2427,6 +2492,14 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
set_mode(MODE_2PWM2CAP);
break;

case MODE_4PWM1CAP:
set_mode(MODE_4PWM1CAP);
break;

case MODE_5PWM1CAP:
set_mode(MODE_5PWM1CAP);
break;

default:
ret = -EINVAL;
break;
Expand Down Expand Up @@ -2526,11 +2599,28 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6

case PORT_PWM5:
/* select 5-pin PWM mode */
servo_mode = PX4FMU::MODE_5PWM;
break;

case PORT_PWM5CAP1:
/* select 5-pin PWM mode 1 capture */
servo_mode = PX4FMU::MODE_5PWM1CAP;
mode_with_input = true;
break;

case PORT_PWM4:
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
break;

case PORT_PWM4CAP1:
/* select 4-pin PWM mode 1 capture */
servo_mode = PX4FMU::MODE_4PWM1CAP;
mode_with_input = true;
break;

case PORT_PWM3:
/* select 3-pin PWM mode */
servo_mode = PX4FMU::MODE_3PWM;
Expand Down Expand Up @@ -2912,6 +3002,12 @@ int PX4FMU::custom_command(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm3")) {
new_mode = PORT_PWM3;

} else if (!strcmp(verb, "mode_pwm5cap1")) {
new_mode = PORT_PWM5CAP1;

} else if (!strcmp(verb, "mode_pwm4cap1")) {
new_mode = PORT_PWM4CAP1;

} else if (!strcmp(verb, "mode_pwm3cap1")) {
new_mode = PORT_PWM3CAP1;

Expand Down Expand Up @@ -3049,7 +3145,11 @@ int PX4FMU::print_status()

case MODE_2PWM2CAP: mode_str = "pwm2cap2"; break;

case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break;
case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break;

case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break;

case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break;

case MODE_6PWM: mode_str = "pwm6"; break;

Expand Down

0 comments on commit 60d0a08

Please sign in to comment.