From c7edd9f31b24117fbc56a6e3f36189ad4d68ad5f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 25 Sep 2018 14:38:38 -0700 Subject: [PATCH] fmu:Add MODE_4PWM1CAP, MODE_5PWM, MODE_5PWM1CAP This extends the Capture support for FMU CHAN 5 and 6. --- src/drivers/drv_pwm_output.h | 19 ++++--- src/drivers/px4fmu/fmu.cpp | 104 ++++++++++++++++++++++++++++++++++- 2 files changed, 113 insertions(+), 10 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 3adf6f12f0..a104b9de2e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -259,14 +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_ENTER_TEST_MODE 13 -#define PWM_SERVO_EXIT_TEST_MODE 14 +#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) /* diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ffbdab582f..82eacadf53 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -84,11 +84,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, }; @@ -108,6 +111,9 @@ public: MODE_3PWM, MODE_3PWM1CAP, MODE_4PWM, + MODE_4PWM1CAP, + MODE_5PWM, + MODE_5PWM1CAP, MODE_6PWM, MODE_8PWM, MODE_14PWM, @@ -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 PX4_DEBUG("MODE_4PWM"); @@ -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: @@ -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 @@ -1936,7 +1976,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; @@ -1988,6 +2034,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: @@ -2040,6 +2090,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; @@ -2346,7 +2408,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; } @@ -2421,6 +2484,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; @@ -2448,6 +2513,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; @@ -2547,11 +2620,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; @@ -2944,6 +3034,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; @@ -3081,7 +3177,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;