CF2 correct motor map and LEDs

src/drivers/boards/crazyflie/board_config.h

@@ -75,9 +75,11 @@ __BEGIN_DECLS
 #define GPIO_LED_RED_R       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
 			      GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
 
-/* Blinking while charging */
+/* PX4: armed state indicator ; Stock FW: Blinking while charging */
-#define GPIO_LED_BLUE_L		(GPIO_OUTPUT|GPIO_PORTD|GPIO_PIN2)
+#define GPIO_LED_BLUE_L		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN2)
 
+#define LED_TX 4
+#define LED_RX 5
 
 #define GPIO_FSYNC_MPU9250		(GPIO_OUTPUT|GPIO_PORTC|GPIO_PIN14) // Needs to be set low
 #define GPIO_DRDY_MPU9250		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN13)

src/drivers/boards/crazyflie/crazyflie_init.c

@@ -94,6 +94,19 @@
 #  endif
 #endif
 
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
 /****************************************************************************
  * Protected Functions
  ****************************************************************************/
@@ -169,6 +182,14 @@ __EXPORT int nsh_archinitialize(void)
 		       (hrt_callout)stm32_serial_dma_poll,
 		       NULL);
 
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_GREEN);
+	led_off(LED_BLUE);
+	led_off(LED_TX);
+	led_off(LED_RX);
+
 
 
 	result = board_i2c_initialize();

src/drivers/boards/crazyflie/crazyflie_led.c

@@ -37,7 +37,7 @@
  * Crazyflie LED backend.
  */
 
-#include <nuttx/config.h>
+#include <px4_config.h>
 
 #include <stdbool.h>
 
@@ -60,37 +60,42 @@ extern void led_off(int led);
 extern void led_toggle(int led);
 __END_DECLS
 
+static uint32_t g_ledmap[] = {
+	GPIO_LED_BLUE_L,  // Indexed by LED_BLUE
+	GPIO_LED_RED_R,   // Indexed by LED_RED, LED_AMBER
+	0,				  // Indexed by LED_SAFETY
+	GPIO_LED_GREEN_R, // Indexed by LED_GREEN
+	GPIO_LED_RED_L,   // Indexed by LED_TX
+	GPIO_LED_GREEN_L  // Indexed by LED_RX
+};
+
+
 __EXPORT void led_init()
 {
 	/* Configure LED1 GPIO for output */
-
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
-	stm32_configgpio(GPIO_LED_RED_L);
+		px4_arch_configgpio(g_ledmap[l]);
+	}
 }
 
 __EXPORT void led_on(int led)
 {
-	if (led == 1) {
 	/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED_RED_L, false);
+	px4_arch_gpiowrite(g_ledmap[led], g_ledmap[led] & GPIO_OUTPUT_SET ? true : false);
-	}
 }
 
 __EXPORT void led_off(int led)
 {
-	if (led == 1) {
 	/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED_RED_L, true);
+	px4_arch_gpiowrite(g_ledmap[led], g_ledmap[led] & GPIO_OUTPUT_SET ? false : true);
-	}
 }
 
 __EXPORT void led_toggle(int led)
 {
-	if (led == 1) {
+	if (px4_arch_gpioread(g_ledmap[led])) {
-		if (stm32_gpioread(GPIO_LED_RED_L)) {
+		px4_arch_gpiowrite(g_ledmap[led], false);
-			stm32_gpiowrite(GPIO_LED_RED_L, false);
 
 	} else {
-			stm32_gpiowrite(GPIO_LED_RED_L, true);
+		px4_arch_gpiowrite(g_ledmap[led], true);
-		}
 	}
 }

src/drivers/boards/crazyflie/crazyflie_timer_config.c

@@ -75,6 +75,7 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 
 __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	{
+		// M1
 		.gpio_out = GPIO_TIM2_CH2OUT,
 		.gpio_in = GPIO_TIM2_CH2IN,
 		.timer_index = 0,
@@ -83,14 +84,7 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 		.masks  = GTIM_SR_CC2IF | GTIM_SR_CC2OF
 	},
 	{
-		.gpio_out = GPIO_TIM2_CH4OUT,
+		// M3
-		.gpio_in = GPIO_TIM2_CH4IN,
-		.timer_index = 0,
-		.timer_channel = 4,
-		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
-		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
-	},
-	{
 		.gpio_out = GPIO_TIM2_CH1OUT,
 		.gpio_in = GPIO_TIM2_CH1IN,
 		.timer_index = 0,
@@ -99,11 +93,21 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 		.masks  = GTIM_SR_CC1IF | GTIM_SR_CC1OF
 	},
 	{
+		// M4
 		.gpio_out = GPIO_TIM4_CH4OUT,
 		.gpio_in = GPIO_TIM4_CH4IN,
 		.timer_index = 1,
 		.timer_channel = 4,
 		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
 		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
-	}
+	},
+	{
+		// M2
+		.gpio_out = GPIO_TIM2_CH4OUT,
+		.gpio_in = GPIO_TIM2_CH4IN,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.ccr_offset = STM32_GTIM_CCR4_OFFSET,
+		.masks  = GTIM_SR_CC4IF | GTIM_SR_CC4OF
+	},
 };

src/modules/commander/commander.cpp

@@ -2982,7 +2982,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 		}
 	}
 
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V1) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V1) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) || defined (CONFIG_ARCH_BOARD_CRAZYFLIE)
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
 	if (actuator_armed->armed) {