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

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

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;