From e91821d2a2d6a41a6643b315c65fdfdf6b3aff78 Mon Sep 17 00:00:00 2001
From: Dennis Shtatnov <densht@gmail.com>
Date: Tue, 23 Aug 2016 20:03:52 -0400
Subject: [PATCH] Merge Crazyflie motor driver with FMU
Comment corrections
---
ROMFS/px4fmu_common/init.d/4900_crazyflie | 11 +-
nuttx-configs/crazyflie/include/board.h | 11 +-
src/drivers/boards/crazyflie/CMakeLists.txt | 1 +
src/drivers/boards/crazyflie/board_config.h | 43 +-
src/drivers/boards/crazyflie/crazyflie_init.c | 2 +-
src/drivers/boards/crazyflie/crazyflie_led.c | 12 +-
src/drivers/boards/crazyflie/crazyflie_spi.c | 39 +
src/drivers/crazyflie/CMakeLists.txt | 44 -
src/drivers/crazyflie/crazyflie.cpp | 1100 -----------------
src/drivers/drv_gpio.h | 18 +-
src/drivers/drv_pwm_output.h | 15 +
src/drivers/lps25h/lps25h_i2c.cpp | 15 -
src/drivers/px4fmu/fmu.cpp | 33 +-
src/modules/dummy/tone_alarm.c | 6 +-
src/modules/mavlink/mavlink_main.cpp | 2 +-
src/systemcmds/esc_calib/esc_calib.c | 6 +-
16 files changed, 158 insertions(+), 1200 deletions(-)
create mode 100644 src/drivers/boards/crazyflie/crazyflie_spi.c
delete mode 100644 src/drivers/crazyflie/CMakeLists.txt
delete mode 100644 src/drivers/crazyflie/crazyflie.cpp
diff --git a/ROMFS/px4fmu_common/init.d/4900_crazyflie b/ROMFS/px4fmu_common/init.d/4900_crazyflie
index 9fe3ea47d5..c10a473cf6 100644
--- a/ROMFS/px4fmu_common/init.d/4900_crazyflie
+++ b/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
diff --git a/nuttx-configs/crazyflie/include/board.h b/nuttx-configs/crazyflie/include/board.h
index 05a32dcad7..4ea5dd9072 100644
--- a/nuttx-configs/crazyflie/include/board.h
+++ b/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)
- * PLLM : 24 (STM32_PLLCFG_PLLM)
+ * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL)
+ * 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)
diff --git a/src/drivers/boards/crazyflie/CMakeLists.txt b/src/drivers/boards/crazyflie/CMakeLists.txt
index d74e3d6bc4..f7c5ae68fb 100644
--- a/src/drivers/boards/crazyflie/CMakeLists.txt
+++ b/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
)
diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h
index bba6a77097..582169c207 100644
--- a/src/drivers/boards/crazyflie/board_config.h
+++ b/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|\
- GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
- GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
- GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2)
-#define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
- GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
+/* Radio TX indicator */
+#define GPIO_LED_RED_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+ GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0)
+/* Radio RX indicator */
+#define GPIO_LED_GREEN_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+ GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
+#define GPIO_LED_GREEN_R (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\
+ 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
*
diff --git a/src/drivers/boards/crazyflie/crazyflie_init.c b/src/drivers/boards/crazyflie/crazyflie_init.c
index 64fb9f3c90..3dcb228c90 100644
--- a/src/drivers/boards/crazyflie/crazyflie_init.c
+++ b/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;
}
diff --git a/src/drivers/boards/crazyflie/crazyflie_led.c b/src/drivers/boards/crazyflie/crazyflie_led.c
index b499f7b8e6..e0e3736862 100644
--- a/src/drivers/boards/crazyflie/crazyflie_led.c
+++ b/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)) {
- stm32_gpiowrite(GPIO_LED1, false);
+ if (stm32_gpioread(GPIO_LED_RED_L)) {
+ stm32_gpiowrite(GPIO_LED_RED_L, false);
} else {
- stm32_gpiowrite(GPIO_LED1, true);
+ stm32_gpiowrite(GPIO_LED_RED_L, true);
}
}
}
diff --git a/src/drivers/boards/crazyflie/crazyflie_spi.c b/src/drivers/boards/crazyflie/crazyflie_spi.c
new file mode 100644
index 0000000000..dfe2bb1258
--- /dev/null
+++ b/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 */
+}
diff --git a/src/drivers/crazyflie/CMakeLists.txt b/src/drivers/crazyflie/CMakeLists.txt
deleted file mode 100644
index 5ce9e5beb4..0000000000
--- a/src/drivers/crazyflie/CMakeLists.txt
+++ /dev/null
@@ -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 :
diff --git a/src/drivers/crazyflie/crazyflie.cpp b/src/drivers/crazyflie/crazyflie.cpp
deleted file mode 100644
index 6b3c637e45..0000000000
--- a/src/drivers/crazyflie/crazyflie.cpp
+++ /dev/null
@@ -1,1100 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2012-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.
- *
- ****************************************************************************/
-
-/**
- * @file crazyflie.cpp
- *
- * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
- */
-
-#include <px4_config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <nuttx/arch.h>
-
-#include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
-#include <drivers/drv_hrt.h>
-
-#include <board_config.h>
-
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <systemlib/mixer/mixer.h>
-#include <systemlib/pwm_limit/pwm_limit.h>
-#include <systemlib/board_serial.h>
-#include <systemlib/param/param.h>
-#include <drivers/drv_mixer.h>
-#include <drivers/drv_rc_input.h>
-
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-
-
-#ifdef HRT_PPM_CHANNEL
-# include <systemlib/ppm_decode.h>
-#endif
-
-/*
- * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
- */
-
-#define CONTROL_INPUT_DROP_LIMIT_MS 20
-#define NAN_VALUE (0.0f/0.0f)
-
-#define CRAZYFLIE_DEVICE_PATH PWM_OUTPUT0_DEVICE_PATH
-
-class Crazyflie : public device::CDev
-{
-public:
- Crazyflie();
- virtual ~Crazyflie();
-
- virtual int ioctl(file *filp, int cmd, unsigned long arg);
- virtual ssize_t write(file *filp, const char *buffer, size_t len);
-
- virtual int init();
-
-private:
- static const unsigned _max_actuators = 4;
-
- unsigned _current_update_rate;
- struct work_s _work;
- int _armed_sub;
- int _param_sub;
- orb_advert_t _outputs_pub;
- int _class_instance;
-
- volatile bool _initialized;
- bool _servo_armed;
- bool _pwm_on;
-
- MixerGroup *_mixers;
-
- uint32_t _groups_required;
- uint32_t _groups_subscribed;
- int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
- actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
- orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
- pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
- unsigned _poll_fds_num;
-
- static pwm_limit_t _pwm_limit;
- static actuator_armed_s _armed;
- uint16_t _failsafe_pwm[_max_actuators];
- uint16_t _disarmed_pwm[_max_actuators];
- uint16_t _min_pwm[_max_actuators];
- uint16_t _max_pwm[_max_actuators];
- uint16_t _reverse_pwm_mask;
- unsigned _num_failsafe_set;
- unsigned _num_disarmed_set;
-
- static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
-
- static void cycle_trampoline(void *arg);
-
- void cycle();
- void work_start();
- void work_stop();
-
- static int control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input);
- void subscribe();
- void publish_pwm_outputs(uint16_t *values, size_t numvalues);
-
- void set_bounded_pwm(uint16_t &out, struct pwm_output_values *pwm, size_t i);
-
- /* do not allow to copy due to ptr data members */
- Crazyflie(const Crazyflie &);
- Crazyflie operator=(const Crazyflie &);
-};
-
-pwm_limit_t Crazyflie::_pwm_limit;
-actuator_armed_s Crazyflie::_armed = {};
-
-namespace
-{
-
-Crazyflie *g_crazyflie;
-
-} // namespace
-
-#define CRAZYFLIE_PWM_MIN 0
-#define CRAZYFLIE_PWM_MAX 255
-
-Crazyflie::Crazyflie() :
- CDev("crazyflie", CRAZYFLIE_DEVICE_PATH),
- _current_update_rate(0),
- _work{},
- _armed_sub(-1),
- _param_sub(-1),
- _outputs_pub(nullptr),
- _class_instance(0),
- _initialized(false),
- _servo_armed(false),
- _pwm_on(false),
- _mixers(nullptr),
- _groups_required(0),
- _groups_subscribed(0),
- _control_subs{ -1},
- _poll_fds_num(0),
- _failsafe_pwm{0},
- _disarmed_pwm{0},
- _reverse_pwm_mask(0),
- _num_failsafe_set(0),
- _num_disarmed_set(0)
-{
- for (unsigned i = 0; i < _max_actuators; i++) {
- _min_pwm[i] = CRAZYFLIE_PWM_MIN;
- _max_pwm[i] = CRAZYFLIE_PWM_MAX;
- }
-
- _control_topics[0] = ORB_ID(actuator_controls_0);
- _control_topics[1] = ORB_ID(actuator_controls_1);
- _control_topics[2] = ORB_ID(actuator_controls_2);
- _control_topics[3] = ORB_ID(actuator_controls_3);
-
- memset(_controls, 0, sizeof(_controls));
- memset(_poll_fds, 0, sizeof(_poll_fds));
-
- /* only enable this during development */
- _debug_enabled = false;
-}
-
-Crazyflie::~Crazyflie()
-{
- if (_initialized) {
- /* tell the task we want it to go away */
- work_stop();
-
- int i = 10;
-
- do {
- /* wait 50ms - it should wake every 100ms or so worst-case */
- usleep(50000);
- i--;
-
- } while (_initialized && i > 0);
- }
-
- /* clean up the alternate device node */
- unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
-
- g_crazyflie = nullptr;
-}
-
-int
-Crazyflie::init()
-{
- int ret;
-
- ASSERT(!_initialized);
-
- /* do regular cdev init */
- ret = CDev::init();
-
- if (ret != OK) {
- return ret;
- }
-
- /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
- _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
-
- if (_class_instance == CLASS_DEVICE_PRIMARY) {
- /* lets not be too verbose */
- } else if (_class_instance < 0) {
- warnx("FAILED registering class device");
- }
-
- work_start();
-
- return OK;
-}
-
-void
-Crazyflie::subscribe()
-{
- /* subscribe/unsubscribe to required actuator control groups */
- uint32_t sub_groups = _groups_required & ~_groups_subscribed;
- uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
- _poll_fds_num = 0;
-
- for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
- if (sub_groups & (1 << i)) {
- DEVICE_DEBUG("subscribe to actuator_controls_%d", i);
- _control_subs[i] = orb_subscribe(_control_topics[i]);
- }
-
- if (unsub_groups & (1 << i)) {
- DEVICE_DEBUG("unsubscribe from actuator_controls_%d", i);
- ::close(_control_subs[i]);
- _control_subs[i] = -1;
- }
-
- if (_control_subs[i] > 0) {
- _poll_fds[_poll_fds_num].fd = _control_subs[i];
- _poll_fds[_poll_fds_num].events = POLLIN;
- _poll_fds_num++;
- }
- }
-}
-
-void
-Crazyflie::publish_pwm_outputs(uint16_t *values, size_t numvalues)
-{
- actuator_outputs_s outputs;
- outputs.noutputs = numvalues;
- outputs.timestamp = hrt_absolute_time();
-
- for (size_t i = 0; i < _max_actuators; ++i) {
- outputs.output[i] = i < numvalues ? (float)values[i] : 0;
- }
-
- if (_outputs_pub == nullptr) {
- int instance = -1;
- _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
-
- } else {
- orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
- }
-}
-
-void
-Crazyflie::work_start()
-{
- /* schedule a cycle to start things */
- work_queue(HPWORK, &_work, (worker_t)&Crazyflie::cycle_trampoline, this, 0);
-}
-
-void
-Crazyflie::cycle_trampoline(void *arg)
-{
- Crazyflie *dev = reinterpret_cast<Crazyflie *>(arg);
-
- dev->cycle();
-}
-
-void
-Crazyflie::cycle()
-{
- if (!_initialized) {
- /* force a reset of the update rate */
- _current_update_rate = 0;
-
- _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
- _param_sub = orb_subscribe(ORB_ID(parameter_update));
-
- /* initialize PWM limit lib */
- pwm_limit_init(&_pwm_limit);
-
- up_pwm_servo_init(0x0f);
-
- _initialized = true;
- }
-
-
- if (_groups_subscribed != _groups_required) {
- subscribe();
- _groups_subscribed = _groups_required;
- /* force setting update rate */
- _current_update_rate = 0;
- }
-
- /*
- * Adjust actuator topic update rate to keep up with
- * the highest servo update rate configured.
- *
- * We always mix at max rate; some channels may update slower.
- */
- unsigned max_rate = 500;
-
- if (_current_update_rate != max_rate) {
- _current_update_rate = max_rate;
- int update_rate_in_ms = int(1000 / _current_update_rate);
-
- /* reject faster than 500 Hz updates */
- if (update_rate_in_ms < 2) {
- update_rate_in_ms = 2;
- }
-
- /* reject slower than 10 Hz updates */
- if (update_rate_in_ms > 100) {
- update_rate_in_ms = 100;
- }
-
- DEVICE_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms);
-
- for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
- if (_control_subs[i] > 0) {
- orb_set_interval(_control_subs[i], update_rate_in_ms);
- }
- }
-
- // set to current max rate, even if we are actually checking slower/faster
- _current_update_rate = max_rate;
- }
-
- /* check if anything updated */
- int ret = ::poll(_poll_fds, _poll_fds_num, 0);
-
- /* this would be bad... */
- if (ret < 0) {
- DEVICE_LOG("poll error %d", errno);
-
- } else if (ret == 0) {
- /* timeout: no control data, switch to failsafe values */
-// warnx("no PWM: failsafe");
-
- } else {
-
- /* get controls for required topics */
- unsigned poll_id = 0;
-
- for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
- if (_control_subs[i] > 0) {
- if (_poll_fds[poll_id].revents & POLLIN) {
- orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
- }
-
- poll_id++;
- }
- }
-
- /* can we mix? */
- if (_mixers != nullptr) {
-
- size_t num_outputs = 4;
-
- /* do mixing */
- float outputs[_max_actuators];
- num_outputs = _mixers->mix(outputs, num_outputs, NULL);
-
- /* disable unused ports by setting their output to NaN */
- for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
- if (i >= num_outputs) {
- outputs[i] = NAN_VALUE;
- }
- }
-
- uint16_t pwm_limited[_max_actuators];
-
- /* the PWM limit call takes care of out of band errors, NaN and constrains */
- pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
- outputs, pwm_limited, &_pwm_limit);
-
- /* output to the servos */
- for (size_t i = 0; i < num_outputs; i++) {
- up_pwm_servo_set(i, pwm_limited[i]);
- }
-
- publish_pwm_outputs(pwm_limited, num_outputs);
- }
- }
-
- /* check arming state */
- bool updated = false;
- orb_check(_armed_sub, &updated);
-
- if (updated) {
- orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
-
- /* update the armed status and check that we're not locked down */
- bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
-
- if (_servo_armed != set_armed) {
- _servo_armed = set_armed;
- }
-
- /* update PWM status if armed or if disarmed PWM values are set */
- bool pwm_on = (set_armed || _num_disarmed_set > 0);
-
- if (_pwm_on != pwm_on) {
- _pwm_on = pwm_on;
- up_pwm_servo_arm(pwm_on);
- }
- }
-
- orb_check(_param_sub, &updated);
-
- if (updated) {
- parameter_update_s pupdate;
- orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
- }
-
- work_queue(HPWORK, &_work, (worker_t)&Crazyflie::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000));
-}
-
-void Crazyflie::work_stop()
-{
- work_cancel(HPWORK, &_work);
-
- for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
- if (_control_subs[i] > 0) {
- ::close(_control_subs[i]);
- _control_subs[i] = -1;
- }
- }
-
- ::close(_armed_sub);
- ::close(_param_sub);
-
- /* make sure servos are off */
- up_pwm_servo_deinit();
-
- DEVICE_LOG("stopping");
-
- /* note - someone else is responsible for restoring the GPIO config */
-
- /* tell the dtor that we are exiting */
- _initialized = false;
-}
-
-int
-Crazyflie::control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input)
-{
- const actuator_controls_s *controls = (actuator_controls_s *)handle;
-
- input = controls[control_group].control[control_index];
-
- /* limit control input */
- if (input > 1.0f) {
- input = 1.0f;
-
- } else if (input < -1.0f) {
- input = -1.0f;
- }
-
- /* motor spinup phase - lock throttle to zero */
- if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
- if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
- control_index == actuator_controls_s::INDEX_THROTTLE) {
- /* limit the throttle output to zero during motor spinup,
- * as the motors cannot follow any demand yet
- */
- input = 0.0f;
- }
- }
-
- /* throttle not arming - mark throttle input as invalid */
- if (arm_nothrottle()) {
- if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
- control_index == actuator_controls_s::INDEX_THROTTLE) {
- /* set the throttle to an invalid value */
- input = NAN_VALUE;
- }
- }
-
- return 0;
-}
-
-void
-Crazyflie::set_bounded_pwm(uint16_t &out, struct pwm_output_values *pwm, size_t i)
-{
- if (pwm->values[i] > 255) {
- out = 255;
-
- } else {
- out = pwm->values[i];
- }
-}
-
-int
-Crazyflie::ioctl(file *filp, int cmd, unsigned long arg)
-{
- int ret = OK;
-
- lock();
-
- switch (cmd) {
- case PWM_SERVO_ARM:
- up_pwm_servo_arm(true);
- break;
-
- case PWM_SERVO_SET_ARM_OK:
- case PWM_SERVO_CLEAR_ARM_OK:
- case PWM_SERVO_SET_FORCE_SAFETY_OFF:
- case PWM_SERVO_SET_FORCE_SAFETY_ON:
- // these are no-ops, as no safety switch
- break;
-
- case PWM_SERVO_DISARM:
- up_pwm_servo_arm(false);
- break;
-
- case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
- case PWM_SERVO_GET_UPDATE_RATE:
- *(uint32_t *)arg = 500;
- break;
-
- case PWM_SERVO_SET_UPDATE_RATE:
- case PWM_SERVO_SET_SELECT_UPDATE_RATE:
- // these are no-ops, as we have a fixed update rate
- break;
-
- case PWM_SERVO_GET_SELECT_UPDATE_RATE:
- *(uint32_t *)arg = (1 << 4) - 1;
- break;
-
- case PWM_SERVO_SET_FAILSAFE_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
- break;
- }
-
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- set_bounded_pwm(_failsafe_pwm[i], pwm, i);
- }
-
- /*
- * update the counter
- * this is needed to decide if disarmed PWM output should be turned on or not
- */
- _num_failsafe_set = 0;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- if (_failsafe_pwm[i] > 0) {
- _num_failsafe_set++;
- }
- }
-
- break;
- }
-
- case PWM_SERVO_GET_FAILSAFE_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _failsafe_pwm[i];
- }
-
- pwm->channel_count = _max_actuators;
- break;
- }
-
- case PWM_SERVO_SET_DISARMED_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
- break;
- }
-
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- set_bounded_pwm(_disarmed_pwm[i], pwm, i);
- }
-
- /*
- * update the counter
- * this is needed to decide if disarmed PWM output should be turned on or not
- */
- _num_disarmed_set = 0;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- if (_disarmed_pwm[i] > 0) {
- _num_disarmed_set++;
- }
- }
-
- break;
- }
-
- case PWM_SERVO_GET_DISARMED_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _disarmed_pwm[i];
- }
-
- pwm->channel_count = _max_actuators;
- break;
- }
-
- case PWM_SERVO_SET_MIN_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
- break;
- }
-
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- set_bounded_pwm(_min_pwm[i], pwm, i);
- }
-
- break;
- }
-
- case PWM_SERVO_GET_MIN_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _min_pwm[i];
- }
-
- pwm->channel_count = _max_actuators;
- arg = (unsigned long)&pwm;
- break;
- }
-
- case PWM_SERVO_SET_MAX_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- /* discard if too many values are sent */
- if (pwm->channel_count > _max_actuators) {
- ret = -EINVAL;
- break;
- }
-
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- set_bounded_pwm(_max_pwm[i], pwm, i);
- }
-
- break;
- }
-
- case PWM_SERVO_GET_MAX_PWM: {
- struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
-
- for (unsigned i = 0; i < _max_actuators; i++) {
- pwm->values[i] = _max_pwm[i];
- }
-
- pwm->channel_count = _max_actuators;
- arg = (unsigned long)&pwm;
- break;
- }
-
- case PWM_SERVO_SET(3):
- case PWM_SERVO_SET(2):
- case PWM_SERVO_SET(1):
- case PWM_SERVO_SET(0):
- if (arg <= 255) {
- up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
-
- } else {
- ret = -EINVAL;
- }
-
- break;
-
- case PWM_SERVO_GET(3):
- case PWM_SERVO_GET(2):
- case PWM_SERVO_GET(1):
- case PWM_SERVO_GET(0):
- *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
- break;
-
- case PWM_SERVO_GET_RATEGROUP(0):
- case PWM_SERVO_GET_RATEGROUP(1):
- case PWM_SERVO_GET_RATEGROUP(2):
- case PWM_SERVO_GET_RATEGROUP(3):
- case PWM_SERVO_GET_RATEGROUP(4):
- case PWM_SERVO_GET_RATEGROUP(5):
- *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
- break;
-
- case PWM_SERVO_GET_COUNT:
- case MIXERIOCGETOUTPUTCOUNT:
- *(unsigned *)arg = 4;
- break;
-
- case PWM_SERVO_SET_COUNT:
- ret = -EINVAL;
-
- case MIXERIOCRESET:
- if (_mixers != nullptr) {
- delete _mixers;
- _mixers = nullptr;
- _groups_required = 0;
- }
-
- break;
-
- case MIXERIOCADDSIMPLE: {
- mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
- SimpleMixer *mixer = new SimpleMixer(control_callback,
- (uintptr_t)_controls, mixinfo);
-
- if (mixer->check()) {
- delete mixer;
- _groups_required = 0;
- ret = -EINVAL;
-
- } else {
- if (_mixers == nullptr)
- _mixers = new MixerGroup(control_callback,
- (uintptr_t)_controls);
-
- _mixers->add_mixer(mixer);
- _mixers->groups_required(_groups_required);
- }
-
- break;
- }
-
- case MIXERIOCLOADBUF: {
- const char *buf = (const char *)arg;
- unsigned buflen = strnlen(buf, 1024);
-
- if (_mixers == nullptr) {
- _mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
- }
-
- if (_mixers == nullptr) {
- _groups_required = 0;
- ret = -ENOMEM;
-
- } else {
-
- ret = _mixers->load_from_buf(buf, buflen);
-
- if (ret != 0) {
- DEVICE_DEBUG("mixer load failed with %d", ret);
- delete _mixers;
- _mixers = nullptr;
- _groups_required = 0;
- ret = -EINVAL;
-
- } else {
-
- _mixers->groups_required(_groups_required);
- }
- }
-
- break;
- }
-
- default:
- ret = -ENOTTY;
- break;
- }
-
- unlock();
-
- return ret;
-}
-
-/*
- this implements PWM output via a write() method, for compatibility
- with px4io
- */
-ssize_t
-Crazyflie::write(file *filp, const char *buffer, size_t len)
-{
- unsigned count = len / 2;
- uint16_t values[4];
-
- if (count > 4) {
- // we have at most 4 outputs
- count = 4;
- }
-
- // allow for misaligned values
- memcpy(values, buffer, count * 2);
-
- for (uint8_t i = 0; i < count; i++) {
- if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
- up_pwm_servo_set(i, values[i]);
- }
- }
-
- return count * 2;
-}
-
-namespace
-{
-int crazyflie_start(void);
-int crazyflie_stop(void);
-void test(void);
-void fake(int argc, char *argv[]);
-
-int
-crazyflie_start(void)
-{
- int ret = OK;
-
- if (g_crazyflie == nullptr) {
-
- g_crazyflie = new Crazyflie;
-
- if (g_crazyflie == nullptr) {
- ret = -ENOMEM;
-
- } else {
- ret = g_crazyflie->init();
-
- if (ret != OK) {
- delete g_crazyflie;
- g_crazyflie = nullptr;
- }
- }
- }
-
- return ret;
-}
-
-int
-crazyflie_stop(void)
-{
- int ret = OK;
-
- if (g_crazyflie != nullptr) {
-
- delete g_crazyflie;
- g_crazyflie = nullptr;
- }
-
- return ret;
-}
-
-void
-test(void)
-{
- int fd;
- unsigned servo_count = 0;
- unsigned pwm_value = 1;
- int direction = 1;
- int ret;
-
- fd = open(CRAZYFLIE_DEVICE_PATH, O_RDWR);
-
- if (fd < 0) {
- errx(1, "open fail");
- }
-
- if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) { err(1, "servo arm failed"); }
-
- if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
- err(1, "Unable to get servo count\n");
- }
-
- warnx("Testing %u motors", (unsigned)servo_count);
-
- struct pollfd fds;
- fds.fd = 0; /* stdin */
- fds.events = POLLIN;
-
- warnx("Press CTRL-C or 'c' to abort.");
-
- for (;;) {
- /* sweep all servos between 0..20 */
- servo_position_t servos[servo_count];
-
- for (unsigned i = 0; i < servo_count; i++) {
- servos[i] = pwm_value;
- }
-
- if (direction == 1) {
- // use ioctl interface for one direction
- for (unsigned i = 0; i < servo_count; i++) {
- if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
- err(1, "servo %u set failed", i);
- }
- }
-
- } else {
- // and use write interface for the other direction
- ret = write(fd, servos, sizeof(servos));
-
- if (ret != (int)sizeof(servos)) {
- err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
- }
- }
-
- if (direction > 0) {
- if (pwm_value < 20) {
- pwm_value++;
-
- } else {
- direction = -1;
- }
-
- } else {
- if (pwm_value > 1) {
- pwm_value--;
-
- } else {
- direction = 1;
- }
- }
-
- /* readback servo values */
- for (unsigned i = 0; i < servo_count; i++) {
- servo_position_t value;
-
- if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
- err(1, "error reading PWM servo %d", i);
- }
-
- if (value != servos[i]) {
- errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
- }
- }
-
- /* Check if user wants to quit */
- char c;
- ret = poll(&fds, 1, 0);
-
- if (ret > 0) {
-
- read(0, &c, 1);
-
- if (c == 0x03 || c == 0x63 || c == 'q') {
- warnx("User abort\n");
- break;
- }
- }
-
- usleep(50000);
- }
-
- /* zero motors */
- servo_position_t servos[servo_count];
-
- for (unsigned i = 0; i < servo_count; i++) {
- servos[i] = 0;
- }
-
- for (unsigned i = 0; i < servo_count; i++) {
- if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
- err(1, "servo %u set failed", i);
- }
- }
-
- close(fd);
-
- exit(0);
-}
-
-void
-fake(int argc, char *argv[])
-{
- if (argc < 5) {
- errx(1, "crazyflie fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
- }
-
- actuator_controls_s ac;
-
- ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
-
- ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
-
- ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
-
- ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
-
- orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
-
- if (handle == nullptr) {
- errx(1, "advertise failed");
- }
-
- actuator_armed_s aa;
-
- aa.armed = true;
- aa.lockdown = false;
-
- handle = orb_advertise(ORB_ID(actuator_armed), &aa);
-
- if (handle == nullptr) {
- errx(1, "advertise failed 2");
- }
-
- exit(0);
-}
-
-} // namespace
-
-extern "C" __EXPORT int crazyflie_main(int argc, char *argv[]);
-
-int
-crazyflie_main(int argc, char *argv[])
-{
- const char *verb = argv[1];
-
- if (!strcmp(verb, "stop")) {
- crazyflie_stop();
- errx(0, "Crazyflie driver stopped");
- }
-
- if (!strcmp(verb, "start")) {
- if (crazyflie_start() == OK) {
- errx(0, "Crazyflie driver started");
-
- } else {
- errx(1, "failed to start the Crazyflie driver");
- }
-
- }
-
- // TODO: Ensure driver is running
-
- if (!strcmp(verb, "test")) {
- test();
- }
-
- if (!strcmp(verb, "fake")) {
- fake(argc - 1, argv + 1);
- }
-
- fprintf(stderr, "Crazyflie: unrecognised command %s, try:\n", verb);
- fprintf(stderr, " test, fake\n");
- exit(1);
-}
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index f296bd8eb9..d6cf297226 100644
--- a/src/drivers/drv_gpio.h
+++ b/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
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 099f1c945a..4d4cff6df0 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/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
*/
diff --git a/src/drivers/lps25h/lps25h_i2c.cpp b/src/drivers/lps25h/lps25h_i2c.cpp
index 5bd364a74b..c5111e319b 100644
--- a/src/drivers/lps25h/lps25h_i2c.cpp
+++ b/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);
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 1de83689a4..523be4b022 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/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) {
+ }
+
+#if PWM_LOWEST_MIN > 0
+
+ else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
+ }
- } else {
+#endif
+
+ else {
_min_pwm[i] = pwm->values[i];
}
}
diff --git a/src/modules/dummy/tone_alarm.c b/src/modules/dummy/tone_alarm.c
index cd19ddeb91..3fed639d00 100644
--- a/src/modules/dummy/tone_alarm.c
+++ b/src/modules/dummy/tone_alarm.c
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
- * Author: Anton Babushkin <anton.babushkin@me.com>
+ * Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ * 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>
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 189e617812..a27361b551 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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 */
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index 161fe7923b..913d38c8bf 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/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;
}
--
GitLab