yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

fmu:Add MODE_4PWM1CAP, MODE_5PWM, MODE_5PWM1CAP

Browse Source 
This extends the  Capture support for FMU
   CHAN 5 and 6.
This commit is contained in:
David Sidrane
2018-09-25 14:38:38 -07:00
committed by Lorenz Meier
parent 719bfd1073
commit c7edd9f31b
2 changed files with 113 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/drivers/drv_pwm_output.h
View File

@@ -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)
/*

104
src/drivers/px4fmu/fmu.cpp
View File

@@ -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;