From b99b8865764eccb732c12b8c817184c36426338f Mon Sep 17 00:00:00 2001
From: Dennis Shtatnov <densht@gmail.com>
Date: Sat, 27 Aug 2016 08:07:48 -0400
Subject: [PATCH] CF2 correct motor map and LEDs
---
src/drivers/boards/crazyflie/board_config.h | 6 ++-
src/drivers/boards/crazyflie/crazyflie_init.c | 21 ++++++++++
src/drivers/boards/crazyflie/crazyflie_led.c | 39 +++++++++++--------
.../boards/crazyflie/crazyflie_timer_config.c | 22 ++++++-----
src/modules/commander/commander.cpp | 2 +-
5 files changed, 61 insertions(+), 29 deletions(-)
diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h
index 582169c207..04b7c693b7 100644
--- a/src/drivers/boards/crazyflie/board_config.h
+++ b/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 */
-#define GPIO_LED_BLUE_L (GPIO_OUTPUT|GPIO_PORTD|GPIO_PIN2)
+/* PX4: armed state indicator ; Stock FW: Blinking while charging */
+#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)
diff --git a/src/drivers/boards/crazyflie/crazyflie_init.c b/src/drivers/boards/crazyflie/crazyflie_init.c
index 3dcb228c90..f22bc48838 100644
--- a/src/drivers/boards/crazyflie/crazyflie_init.c
+++ b/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();
diff --git a/src/drivers/boards/crazyflie/crazyflie_led.c b/src/drivers/boards/crazyflie/crazyflie_led.c
index e0e3736862..1c70e4766c 100644
--- a/src/drivers/boards/crazyflie/crazyflie_led.c
+++ b/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 */
-
- stm32_configgpio(GPIO_LED_RED_L);
+ for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); 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);
- }
+ /* Pull down to switch on */
+ 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);
- }
+ /* Pull up to switch off */
+ px4_arch_gpiowrite(g_ledmap[led], g_ledmap[led] & GPIO_OUTPUT_SET ? false : true);
}
__EXPORT void led_toggle(int led)
{
- if (led == 1) {
- if (stm32_gpioread(GPIO_LED_RED_L)) {
- stm32_gpiowrite(GPIO_LED_RED_L, false);
+ if (px4_arch_gpioread(g_ledmap[led])) {
+ px4_arch_gpiowrite(g_ledmap[led], false);
- } else {
- stm32_gpiowrite(GPIO_LED_RED_L, true);
- }
+ } else {
+ px4_arch_gpiowrite(g_ledmap[led], true);
}
}
diff --git a/src/drivers/boards/crazyflie/crazyflie_timer_config.c b/src/drivers/boards/crazyflie/crazyflie_timer_config.c
index 936896297c..1f746dee1a 100644
--- a/src/drivers/boards/crazyflie/crazyflie_timer_config.c
+++ b/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,
- .gpio_in = GPIO_TIM2_CH4IN,
- .timer_index = 0,
- .timer_channel = 4,
- .ccr_offset = STM32_GTIM_CCR4_OFFSET,
- .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
- },
- {
+ // M3
.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
+ },
};
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3d96dee758..63768eb326 100644
--- a/src/modules/commander/commander.cpp
+++ b/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) {
--
GitLab