Merge Crazyflie motor driver with FMU

Comment corrections

ROMFS/px4fmu_common/init.d/4900_crazyflie

@@ -4,8 +4,6 @@
 #
 # @type Quadrotor x
 #
-# @maintainer Tim Dyer <dyer.ti@gmail.com>
-#
 
 sh /etc/init.d/4001_quad_x
 
@@ -16,6 +14,13 @@ then
 	param set BAT_N_CELLS 1
 	param set BAT_CAPACITY 240
 	param set BAT_SOURCE 1
+
+	param set PWM_DISARMED 0
+	param set PWM_MIN 0
+	param set PWM_MAX 255
 fi
 
-set OUTPUT_MODE crazyflie
+set PWM_MIN none
+set PWM_MAX none
+set PWM_DISARMED none
+set PWM_RATE 500

nuttx-configs/crazyflie/include/board.h

@@ -56,7 +56,7 @@
  ************************************************************************************/
 
 /* Clocking *************************************************************************/
-/* The PX4FMU uses a 24MHz crystal connected to the HSE.
+/* The Crazyflie 2.0 uses a 8MHz crystal connected to the HSE.
  *
  * This is the canonical configuration:
  *   System Clock source           : PLL (HSE)
@@ -65,8 +65,8 @@
  *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
  *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
  *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
- *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   HSE Frequency(Hz)             : 8000000      (STM32_BOARD_XTAL)
- *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLM                          : 8            (STM32_PLLCFG_PLLM)
  *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
  *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
  *   PLLQ                          : 7            (STM32_PLLCFG_PLLQ)
@@ -81,7 +81,7 @@
 
 /* HSI - 16 MHz RC factory-trimmed
  * LSI - 32 KHz RC
- * HSE - On-board crystal frequency is 24MHz
+ * HSE - On-board crystal frequency is 8MHz
  * LSE - not installed
  */
 
@@ -171,7 +171,6 @@
 #define GPIO_USART3_TX	GPIO_USART3_TX_2
 
 /* NRF51 via syslink */
-// TODO: Flow control?
 #define GPIO_USART6_RX	GPIO_USART6_RX_1
 #define GPIO_USART6_TX	GPIO_USART6_TX_1
 
@@ -198,7 +197,7 @@
 /*
  * SPI
  *
- * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
+ * E_MISO, E_MOSI, E_SCK exposed on headers
  */
 #define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
 #define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)

src/drivers/boards/crazyflie/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_module(
 		crazyflie_led.c
 		crazyflie_timer_config.c
 		crazyflie_i2c.cpp
+		crazyflie_spi.c
 	DEPENDS
 		platforms__common
 	)

src/drivers/boards/crazyflie/board_config.h

@@ -60,19 +60,29 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
-/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/
+/* Crazyflie GPIOs **********************************************************************************/
 /* LEDs */
-// LED1 green, LED2 orange, LED3 red, LED4 blue
 
 
-#define GPIO_LED1       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+/* Radio TX indicator */
-			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0)
+#define GPIO_LED_RED_L       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
-#define GPIO_LED2       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+			      GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0)
-			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
+/* Radio RX indicator */
-#define GPIO_LED3       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+#define GPIO_LED_GREEN_L       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
-			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2)
+				GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
-#define GPIO_LED4       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+#define GPIO_LED_GREEN_R       (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
-			 GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
+				GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2)
+#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)
+
+
+#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)
+
+
 
 /*
  * I2C busses
@@ -128,6 +138,7 @@ __BEGIN_DECLS
 #define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_2
 #define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_2
 #define GPIO_TIM4_CH4OUT	GPIO_TIM4_CH4OUT_1
+#define DIRECT_PWM_OUTPUT_CHANNELS	4
 
 #define GPIO_TIM2_CH2IN		GPIO_TIM2_CH2IN_1
 #define GPIO_TIM2_CH4IN		GPIO_TIM2_CH4IN_2
@@ -140,6 +151,15 @@ __BEGIN_DECLS
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
 
+
+
+#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS
+
+#define BOARD_FMU_GPIO_TAB { {0, 0, 0}, }
+
+
+
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/
@@ -163,9 +183,12 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 
 extern void stm32_spiinitialize(void);
+void board_spi_reset(int ms);
 
 extern void stm32_usbinitialize(void);
 
+extern void board_peripheral_reset(int ms);
+
 /****************************************************************************
  * Name: nsh_archinitialize
  *

src/drivers/boards/crazyflie/crazyflie_init.c

@@ -174,7 +174,7 @@ __EXPORT int nsh_archinitialize(void)
 	result = board_i2c_initialize();
 
 	if (result != OK) {
-		led_on(1);
+		//led_on(1);
 		return -ENODEV;
 	}
 

src/drivers/boards/crazyflie/crazyflie_led.c

@@ -64,14 +64,14 @@ __EXPORT void led_init()
 {
 	/* Configure LED1 GPIO for output */
 
-	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED_RED_L);
 }
 
 __EXPORT void led_on(int led)
 {
 	if (led == 1) {
 		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
+		stm32_gpiowrite(GPIO_LED_RED_L, false);
 	}
 }
 
@@ -79,18 +79,18 @@ __EXPORT void led_off(int led)
 {
 	if (led == 1) {
 		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
+		stm32_gpiowrite(GPIO_LED_RED_L, true);
 	}
 }
 
 __EXPORT void led_toggle(int led)
 {
 	if (led == 1) {
-		if (stm32_gpioread(GPIO_LED1)) {
+		if (stm32_gpioread(GPIO_LED_RED_L)) {
-			stm32_gpiowrite(GPIO_LED1, false);
+			stm32_gpiowrite(GPIO_LED_RED_L, false);
 
 		} else {
-			stm32_gpiowrite(GPIO_LED1, true);
+			stm32_gpiowrite(GPIO_LED_RED_L, true);
 		}
 	}
 }

src/drivers/boards/crazyflie/crazyflie_spi.c

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "board_config.h"
+
+__EXPORT void board_spi_reset(int ms)
+{
+	/* Do nothing, no SPI devices used */
+}

src/drivers/crazyflie/CMakeLists.txt

@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__crazyflie
-	MAIN crazyflie
-	STACK 1200
-	COMPILE_FLAGS
-		-Os
-	SRCS
-		crazyflie.cpp
-	DEPENDS
-		platforms__common
-	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/drivers/drv_gpio.h

@@ -143,6 +143,20 @@
 # define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE
+
+# define GPIO_SERVO_1			(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2			(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3			(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4			(1<<3)		/**< servo 4 output */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /* no GPIO driver on the PX4IOv1 board */
 #endif
@@ -159,10 +173,6 @@
 /* no GPIO driver on the ASC board */
 #endif
 
-#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE
-/* no GPIO driver on the CRAZYFLIE board */
-#endif
-
 #ifdef CONFIG_ARCH_BOARD_SITL
 /* no GPIO driver on the SITL configuration */
 #endif

src/drivers/drv_pwm_output.h

@@ -73,6 +73,19 @@ __BEGIN_DECLS
  */
 //#define PWM_OUTPUT_MAX_CHANNELS	16
 
+#if defined(CONFIG_ARCH_BOARD_CRAZYFLIE)
+
+/* PWM directly wired to transistor. Duty cycle directly corresponds to power */
+#define PWM_LOWEST_MIN 0
+#define PWM_MOTOR_OFF	0
+#define PWM_DEFAULT_MIN 0
+#define PWM_HIGHEST_MIN 0
+#define PWM_HIGHEST_MAX 255
+#define PWM_DEFAULT_MAX 255
+#define PWM_LOWEST_MAX 255
+
+#else
+
 /**
  * Lowest minimum PWM in us
  */
@@ -108,6 +121,8 @@ __BEGIN_DECLS
  */
 #define PWM_LOWEST_MAX 200
 
+#endif
+
 /**
  * Do not output a channel with this value
  */

src/drivers/lps25h/lps25h_i2c.cpp

@@ -109,21 +109,6 @@ LPS25H_I2C::ioctl(unsigned operation, unsigned &arg)
 
 	switch (operation) {
 
-	case MAGIOCGEXTERNAL:
-// On PX4v1 the MAG can be on an internal I2C
-// On everything else its always external
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-		if (_bus == PX4_I2C_BUS_EXPANSION) {
-			return 1;
-
-		} else {
-			return 0;
-		}
-
-#else
-		return 1;
-#endif
-
 	case DEVIOCGDEVICEID:
 		return CDev::ioctl(nullptr, operation, arg);
 

src/drivers/px4fmu/fmu.cpp

@@ -1628,10 +1628,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
 					_failsafe_pwm[i] = PWM_HIGHEST_MAX;
 
-				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				}
+
+#if PWM_LOWEST_MIN > 0
+
+				else if (pwm->values[i] < PWM_LOWEST_MIN) {
 					_failsafe_pwm[i] = PWM_LOWEST_MIN;
 
-				} else {
+				}
+
+#endif
+
+				else {
 					_failsafe_pwm[i] = pwm->values[i];
 				}
 			}
@@ -1676,11 +1684,17 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					/* ignore 0 */
 				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
 					_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+				}
 
-				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+#if PWM_LOWEST_MIN > 0
+
+				else if (pwm->values[i] < PWM_LOWEST_MIN) {
 					_disarmed_pwm[i] = PWM_LOWEST_MIN;
+				}
 
-				} else {
+#endif
+
+				else {
 					_disarmed_pwm[i] = pwm->values[i];
 				}
 			}
@@ -1726,10 +1740,17 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
 					_min_pwm[i] = PWM_HIGHEST_MIN;
 
-				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				}
-					_min_pwm[i] = PWM_LOWEST_MIN;
 
-				} else {
+#if PWM_LOWEST_MIN > 0
+
+				else if (pwm->values[i] < PWM_LOWEST_MIN) {
+					_min_pwm[i] = PWM_LOWEST_MIN;
+				}
+
+#endif
+
+				else {
 					_min_pwm[i] = pwm->values[i];
 				}
 			}

src/modules/dummy/tone_alarm.c

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Author: Tim Dyer <dyer.ti@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * Dummy tone_alarm to prevent nsh errors.
  *
- * @author Tim Dyer <>
+ * @author Tim Dyer <dyer.ti@gmail.com>
  */
 
 #include <px4_defines.h>

src/modules/mavlink/mavlink_main.cpp

@@ -643,7 +643,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name)
 #endif
 
 #ifndef B1000000
-	#define B1000000 1000000
+#define B1000000 1000000
 #endif
 
 	/* process baud rate */

src/systemcmds/esc_calib/esc_calib.c

@@ -159,7 +159,11 @@ esc_calib_main(int argc, char *argv[])
 			/* Read in custom low value */
 			pwm_low = strtoul(myoptarg, &ep, 0);
 
-			if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) {
+			if (*ep != '\0'
+#if PWM_LOWEST_MIN > 0
+			    || pwm_low < PWM_LOWEST_MIN
+#endif
+			    || pwm_low > PWM_HIGHEST_MIN) {
 				usage("low PWM invalid");
 				return 1;
 			}