diff options
Diffstat (limited to 'src')
127 files changed, 8016 insertions, 4062 deletions
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 62c0d1f17..c341aa2c6 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - float _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 01b89c8fa..81f634992 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -46,7 +46,6 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls_effective.h> #include <systemlib/err.h> #include "ardrone_motor_control.h" @@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ static bool initialized = false; - /* publish effective outputs */ - static struct actuator_controls_effective_s actuator_controls_effective; - static orb_advert_t actuator_controls_effective_pub; /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { @@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - - - /* publish effective outputs */ - actuator_controls_effective.control_effective[0] = roll_control; - actuator_controls_effective.control_effective[1] = pitch_control; - /* yaw output after limiting */ - actuator_controls_effective.control_effective[2] = yaw_control; - /* possible motor thrust limiting */ - actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; - - if (!initialized) { - /* advertise and publish */ - actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); - initialized = true; - } else { - /* already initialized, just publishing */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); - } - /* set the motor values */ /* scale up from 0..1 to 10..500) */ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde499..7ab63953f 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -79,17 +79,37 @@ __BEGIN_DECLS #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) + +/* Data ready pins off */ +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) + +/* SPI1 chip selects off */ +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f7..269ec238e 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ @@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 09838d02f..c66c490a7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral @@ -81,6 +82,12 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 @@ -99,6 +106,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: @@ -106,6 +114,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: @@ -113,6 +122,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a..7954ce5ab 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -109,6 +109,42 @@ CDev::~CDev() } int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + +int CDev::init() { // base class init first diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c..0235f6284 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d64..fed6c312c 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return OK; } +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2e..299575dc5 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -102,6 +102,17 @@ protected: int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency); + + /** * Locking modes supported by the driver. */ enum LockMode { diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 37af26d52..f60964c2b 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -46,7 +46,7 @@ /* * PX4FMU GPIO numbers. * - * For shared pins, alternate function 1 selects the non-GPIO mode + * For shared pins, alternate function 1 selects the non-GPIO mode * (USART2, CAN2, etc.) */ # define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */ @@ -144,4 +144,6 @@ /** read all the GPIOs and return their values in *(uint32_t *)arg */ #define GPIO_GET GPIOC(12) +#define GPIO_SENSOR_RAIL_RESET GPIOC(13) + #endif /* _DRV_GPIO_H */
\ No newline at end of file diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca7..d130d68b3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -142,6 +142,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); __EXPORT extern void hrt_cancel(struct hrt_call *entry); /* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + +/* * Initialise the HRT. */ __EXPORT extern void hrt_init(void); diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 78ffad9d7..03f1dfbe5 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -60,7 +60,7 @@ /** * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 18 +#define RC_INPUT_MAX_CHANNELS 20 /** * Input signal type, value is a control position from zero to 100 @@ -89,6 +89,9 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ + int32_t rssi; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff8..d3b99ae66 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -154,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -315,12 +317,13 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -351,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -374,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -875,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1256,7 +1268,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,10 +1300,10 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1388,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1413,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index bb8d45bea..90a744015 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), _home_sub, &home); - _home_lat = ((double)(home.lat))*1e-7d; - _home_lon = ((double)(home.lon))*1e-7d; + _home_lat = home.lat; + _home_lon = home.lon; _home_position_set = true; } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f5674823..670e51b97 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -92,9 +94,17 @@ static const int ERROR = -1; #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -191,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -198,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -219,6 +232,11 @@ private: void reset(); /** + * disable I2C on the chip + */ + void disable_i2c(); + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -298,16 +316,19 @@ private: }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) @@ -333,8 +354,13 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -352,10 +378,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -574,6 +603,7 @@ L3GD20::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -653,16 +683,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_20HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } @@ -700,12 +729,30 @@ L3GD20::stop() } void +L3GD20::disable_i2c(void) +{ + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); +} + +void L3GD20::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -716,7 +763,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(L3GD20_DEFAULT_RATE); + set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); @@ -732,9 +779,26 @@ L3GD20::measure_trampoline(void *arg) dev->measure(); } +#ifdef GPIO_EXTI_GYRO_DRDY +# define L3GD20_USE_DRDY 1 +#else +# define L3GD20_USE_DRDY 0 +#endif + void L3GD20::measure() { +#if L3GD20_USE_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -753,9 +817,20 @@ L3GD20::measure() perf_begin(_sample_perf); /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#if L3GD20_USE_DRDY + if ((raw_report.status & 0xF) != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -833,6 +908,8 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); + perf_print_counter(_reschedules); + perf_print_counter(_errors); _reports->print_info("report queue"); } @@ -883,7 +960,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -892,7 +969,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -900,6 +977,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -924,10 +1003,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -948,6 +1027,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -960,7 +1041,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -971,6 +1052,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c..969b5e25f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include <nuttx/config.h> #include <sys/types.h> +#include <sys/stat.h> #include <stdint.h> #include <stdbool.h> #include <stddef.h> @@ -63,6 +64,7 @@ #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> #include <drivers/device/ringbuffer.h> +#include <drivers/drv_tone_alarm.h> #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> @@ -78,23 +80,29 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D - -#define ADDR_OUT_TEMP_A 0x26 +#define WHO_I_AM 0x49 + +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e + #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 @@ -112,6 +120,26 @@ static const int ERROR = -1; #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) @@ -201,6 +229,21 @@ public: */ void print_info(); + /** + * dump register values + */ + void print_registers(); + + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -234,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -243,6 +286,8 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -253,6 +298,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -271,6 +325,11 @@ private: void reset(); /** + * disable I2C on the chip + */ + void disable_i2c(); + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -408,6 +467,8 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class LSM303D; @@ -415,6 +476,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -422,7 +486,7 @@ private: LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), @@ -436,18 +500,26 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -479,11 +551,17 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -505,10 +583,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -516,26 +590,45 @@ LSM303D::init() reset(); - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; + } - if (mag_ret != OK) { - _mag_topic = -1; + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - ret = OK; out: return ret; } void +LSM303D::disable_i2c(void) +{ + uint8_t a = read_reg(0x02); + write_reg(0x02, (0x10 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xF7 & a)); + a = read_reg(0x15); + write_reg(0x15, (0x80 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xE7 & a)); +} + +void LSM303D::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); @@ -544,10 +637,17 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); @@ -572,6 +672,122 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + static bool boot_ok = false; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } else { + boot_ok = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (_last_log_us == 0 || + now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw, + (unsigned)boot_ok); + } + + const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; + uint8_t regval[sizeof(reglist)]; + for (uint8_t i=0; i<sizeof(reglist); i++) { + regval[i] = read_reg(reglist[i]); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if (_last_log_reg_us == 0 || + (is_extreme && (now - _last_log_reg_us > 250*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i<sizeof(reglist); i++) { + ::dprintf(_accel_log_fd, " %02x:%02x", (unsigned)reglist[i], (unsigned)regval[i]); + } + ::dprintf(_accel_log_fd, "\n"); + } + + // fsync at 0.1Hz + if (now - _last_log_sync_us > 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + uint8_t tone = 3; + if (!is_extreme) { + tone = 3; + } else if (boot_ok) { + tone = 4; + } else { + tone = 5; + } + ::ioctl(tfd, TONE_SET_ALARM, tone); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -590,6 +806,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -952,6 +1169,7 @@ LSM303D::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -1224,6 +1442,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); @@ -1248,6 +1474,7 @@ LSM303D::measure() perf_begin(_accel_sample_perf); /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1290,8 +1517,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1325,6 +1554,7 @@ LSM303D::mag_measure() perf_begin(_mag_sample_perf); /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); @@ -1361,8 +1591,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1380,14 +1612,111 @@ LSM303D::print_info() _mag_reports->print_info("mag reports"); } +void +LSM303D::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_OUT_TEMP_L, "TEMP_L" }, + { ADDR_OUT_TEMP_H, "TEMP_H" }, + { ADDR_INT_CTRL_M, "INT_CTRL_M" }, + { ADDR_INT_SRC_M, "INT_SRC_M" }, + { ADDR_REFERENCE_X, "REFERENCE_X" }, + { ADDR_REFERENCE_Y, "REFERENCE_Y" }, + { ADDR_REFERENCE_Z, "REFERENCE_Z" }, + { ADDR_OUT_X_L_A, "ACCEL_XL" }, + { ADDR_OUT_X_H_A, "ACCEL_XH" }, + { ADDR_OUT_Y_L_A, "ACCEL_YL" }, + { ADDR_OUT_Y_H_A, "ACCEL_YH" }, + { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, + { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, + { ADDR_FIFO_CTRL, "FIFO_CTRL" }, + { ADDR_FIFO_SRC, "FIFO_SRC" }, + { ADDR_IG_CFG1, "IG_CFG1" }, + { ADDR_IG_SRC1, "IG_SRC1" }, + { ADDR_IG_THS1, "IG_THS1" }, + { ADDR_IG_DUR1, "IG_DUR1" }, + { ADDR_IG_CFG2, "IG_CFG2" }, + { ADDR_IG_SRC2, "IG_SRC2" }, + { ADDR_IG_THS2, "IG_THS2" }, + { ADDR_IG_DUR2, "IG_DUR2" }, + { ADDR_CLICK_CFG, "CLICK_CFG" }, + { ADDR_CLICK_SRC, "CLICK_SRC" }, + { ADDR_CLICK_THS, "CLICK_THS" }, + { ADDR_TIME_LIMIT, "TIME_LIMIT" }, + { ADDR_TIME_LATENCY,"TIME_LATENCY" }, + { ADDR_TIME_WINDOW, "TIME_WINDOW" }, + { ADDR_ACT_THS, "ACT_THS" }, + { ADDR_ACT_DUR, "ACT_DUR" } + }; + for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) { + printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); + } + printf("_reg1_expected=0x%02x\n", _reg1_expected); + printf("_reg7_expected=0x%02x\n", _reg7_expected); +} + +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1432,6 +1761,8 @@ void start(); void test(); void reset(); void info(); +void regdump(); +void logging(); /** * Start the driver. @@ -1445,7 +1776,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1456,7 +1787,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1464,7 +1795,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1473,6 +1804,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1499,10 +1832,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1528,10 +1861,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) @@ -1554,6 +1887,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1564,7 +1900,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1575,7 +1911,9 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + close(fd); + + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); @@ -1585,6 +1923,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } @@ -1603,6 +1943,35 @@ info() exit(0); } +/** + * dump registers from device + */ +void +regdump() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + exit(0); +} + +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1634,5 +2003,17 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) lsm303d::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "regdump")) + lsm303d::regdump(); + + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6..30d6069b3 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Author: Marco Bauer <marco@wtns.de> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +36,8 @@ * @file mkblctrl.cpp * * Driver/configurator for the Mikrokopter BL-Ctrl. - * Marco Bauer + * + * @author Marco Bauer <marco@wtns.de> * */ @@ -73,7 +75,6 @@ #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/esc_status.h> @@ -89,8 +90,8 @@ #define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F -#define MOTOR_SPINUP_COUNTER 2500 -#define ESC_UORB_PUBLISH_DELAY 200 +#define MOTOR_SPINUP_COUNTER 30 +#define ESC_UORB_PUBLISH_DELAY 500000 class MK : public device::I2C { @@ -112,7 +113,7 @@ public: FRAME_X, }; - MK(int bus); + MK(int bus, const char *_device_path); ~MK(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -126,7 +127,7 @@ public: int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); - unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -141,9 +142,9 @@ private: unsigned int _motor; int _px4mode; int _frametype; + char _device[20]; ///< device orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -244,7 +245,7 @@ MK *g_mk; } // namespace -MK::MK(int bus) : +MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), _mode(MODE_NONE), _update_rate(50), @@ -252,7 +253,6 @@ MK::MK(int bus) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -265,6 +265,10 @@ MK::MK(int bus) : _armed(false), _mixers(nullptr) { + strncpy(_device, _device_path, sizeof(_device)); + /* enforce null termination */ + _device[sizeof(_device) - 1] = '\0'; + _debug_enabled = true; } @@ -291,7 +295,7 @@ MK::~MK() /* clean up the alternate device node */ if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + unregister_driver(_device); g_mk = nullptr; } @@ -313,13 +317,15 @@ MK::init(unsigned motors) usleep(500000); - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } + if (ret == OK) { + log("creating alternate output device"); + _primary_pwm_device = true; + } + + } /* reset GPIOs */ gpio_reset(); @@ -525,13 +531,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -595,9 +594,6 @@ MK::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -633,10 +629,7 @@ MK::task_main() } /* output to BLCtrl's */ - if (_motortest == true) { - mk_servo_test(i); - - } else { + if (_motortest != true) { //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine // 11 Bit Motor[i].SetPoint_PX4 = outputs.output[i]; @@ -668,7 +661,7 @@ MK::task_main() * Only update esc topic every half second. */ - if (hrt_absolute_time() - esc.timestamp > 500000) { + if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) { esc.counter++; esc.timestamp = hrt_absolute_time(); esc.esc_count = (uint8_t) _num_outputs; @@ -692,16 +685,22 @@ MK::task_main() esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; + + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } + } orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } } - //::close(_t_esc_status); + ::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); @@ -727,8 +726,12 @@ MK::mk_servo_arm(bool status) unsigned int -MK::mk_check_for_blctrl(unsigned int count, bool showOutput) +MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { + if(initI2C) { + I2C::init(); + } + _retries = 50; uint8_t foundMotorCount = 0; @@ -952,6 +955,7 @@ MK::mk_servo_test(unsigned int chan) if (_motor >= _num_outputs) { _motor = -1; _motortest = false; + fprintf(stderr, "[mkblctrl] Motortest finished...\n"); } } @@ -1367,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* count used motors */ do { - if (g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { shouldStop = 4; } else { @@ -1377,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, sleep(1); } while (shouldStop < 3); - g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode); @@ -1390,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, } int -mk_start(unsigned bus, unsigned motors) +mk_start(unsigned bus, unsigned motors, char *device_path) { int ret = OK; if (g_mk == nullptr) { - g_mk = new MK(bus); + g_mk = new MK(bus, device_path); if (g_mk == nullptr) { ret = -ENOMEM; @@ -1415,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors) } +int +mk_check_for_i2c_esc_bus(char *device_path, int motors) +{ + int ret; + + if (g_mk == nullptr) { + + g_mk = new MK(3, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 3; + } + + } + + + g_mk = new MK(1, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 1; + } + + } + } + + return -1; +} + + + } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1425,13 +1475,14 @@ mkblctrl_main(int argc, char *argv[]) PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = 1; + int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; + char *devicepath = ""; /* * optional parameters @@ -1491,36 +1542,69 @@ mkblctrl_main(int argc, char *argv[]) newMode = true; } + /* look for the optional device parameter */ + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + devicepath = argv[i + 1]; + newMode = true; + + } else { + errx(1, "missing the devicename (-d)"); + return 1; + } + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); - fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); - fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); + fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Motortest:\n"); + fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); + fprintf(stderr, "mkblctrl -t\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } - if (g_mk == nullptr) { - if (mk_start(bus, motorcount) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - - } else { - newMode = true; - } - } - - - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } - - /* test, etc. here g*/ - - exit(1); + if (!motortest) { + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } + + if (bus != -1) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } + + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } + + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } + + } else { + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); + + } else { + g_mk->set_motor_test(motortest); + exit(0); + + } + } } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110e..bbc595af4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -161,6 +164,14 @@ #define MPU6000_ONE_G 9.80665f +/* + the MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6000_LOW_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -200,17 +211,19 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -338,12 +351,17 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -351,7 +369,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -359,13 +377,15 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -409,8 +429,14 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -455,24 +481,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -480,17 +505,26 @@ out: void MPU6000::reset() { + // if the mpu6000 is initialised after the l3gd20 and lsm303d + // then if we don't do an irqsave/irqrestore here the mpu6000 + // frequenctly comes up in a bad state where all transfers + // come as zero + irqstate_t state; + state = irqsave(); - // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); - // Wake up device and select GyroZ clock (better performance) + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); // Disable I2C bus (recommended on datasheet) write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); + up_udelay(1000); // SAMPLE RATE @@ -652,6 +686,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast<accel_report *>(buffer); int transferred = 0; @@ -669,12 +705,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -746,6 +782,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast<gyro_report *>(buffer); int transferred = 0; @@ -987,9 +1025,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -999,9 +1038,10 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1016,6 +1056,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, nullptr, sizeof(cmd)); } @@ -1139,12 +1182,13 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1159,6 +1203,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ @@ -1249,10 +1307,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1263,19 +1322,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), - _parent(parent) + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void @@ -1331,7 +1419,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1339,6 +1427,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1363,17 +1453,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1431,7 +1521,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1442,6 +1532,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 938786d3f..87788824a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -420,8 +420,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return _reports->size(); case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; case BAROIOCSMSLPRESSURE: diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913b..26216e840 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { @@ -167,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -238,21 +236,31 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..b878d29bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,6 @@ #include <drivers/drv_rc_input.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> @@ -123,7 +122,6 @@ private: int _t_actuators; int _t_actuator_armed; orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -164,6 +162,7 @@ private: static const unsigned _ngpio; void gpio_reset(void); + void sensor_reset(int ms); void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); @@ -219,17 +218,16 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm( {0}), + _disarmed_pwm( {0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -293,11 +291,11 @@ PX4FMU::init() /* start the IO interface task */ _task = task_spawn_cmd("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -326,7 +324,7 @@ PX4FMU::set_mode(Mode mode) switch (mode) { case MODE_2PWM: // v1 multi-port with flow control lines as PWM debug("MODE_2PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -340,7 +338,7 @@ PX4FMU::set_mode(Mode mode) case MODE_4PWM: // v1 multi-port as 4 PWM outs debug("MODE_4PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -354,7 +352,7 @@ PX4FMU::set_mode(Mode mode) case MODE_6PWM: // v2 PWMs as 6 PWM outs debug("MODE_6PWM"); - + /* default output rates */ _pwm_default_rate = 50; _pwm_alt_rate = 50; @@ -396,6 +394,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) continue; @@ -409,6 +408,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate // not a legal map, bail return -EINVAL; } + } else { // set it - errors here are unexpected if (alt != 0) { @@ -416,6 +416,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate warn("rate group set alt failed"); return -EINVAL; } + } else { if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { warn("rate group set default failed"); @@ -425,6 +426,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate } } } + _pwm_alt_rate_channels = rate_map; _pwm_default_rate = default_rate; _pwm_alt_rate = alt_rate; @@ -466,13 +468,6 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -503,6 +498,7 @@ PX4FMU::task_main() * We always mix at max rate; some channels may update slower. */ unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { _current_update_rate = max_rate; int update_rate_in_ms = int(1000 / _current_update_rate); @@ -511,6 +507,7 @@ PX4FMU::task_main() 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; @@ -532,6 +529,7 @@ PX4FMU::task_main() log("poll error %d", errno); usleep(1000000); continue; + } else if (ret == 0) { /* timeout: no control data, switch to failsafe values */ // warnx("no PWM: failsafe"); @@ -542,7 +540,7 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -553,12 +551,15 @@ PX4FMU::task_main() case MODE_2PWM: num_outputs = 2; break; + case MODE_4PWM: num_outputs = 4; break; + case MODE_6PWM: num_outputs = 6; break; + default: num_outputs = 0; break; @@ -572,9 +573,9 @@ PX4FMU::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -588,12 +589,6 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; - } - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); @@ -613,11 +608,13 @@ PX4FMU::task_main() /* update the armed status and check that we're not locked down */ bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) _armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); @@ -626,31 +623,36 @@ PX4FMU::task_main() } #ifdef HRT_PPM_CHANNEL + // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { rc_in.channel_count = RC_INPUT_MAX_CHANNELS; } - for (uint8_t i=0; i<rc_in.channel_count; i++) { + + for (uint8_t i = 0; i < rc_in.channel_count; i++) { rc_in.values[i] = ppm_buffer[i]; } + rc_in.timestamp = ppm_last_valid_decode; /* lazily advertise on first publication */ if (to_input_rc == 0) { to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); - } else { + + } else { orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } + #endif } ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); /* make sure servos are off */ @@ -753,142 +755,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) 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++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _failsafe_pwm[i] = PWM_HIGHEST_MAX; - } else if (pwm->values[i] < PWM_LOWEST_MIN) { - _failsafe_pwm[i] = PWM_LOWEST_MIN; - } else { - _failsafe_pwm[i] = pwm->values[i]; + 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; } - } - /* - * 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++; + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; + + } else { + _failsafe_pwm[i] = pwm->values[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; } - 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; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + + pwm->channel_count = _max_actuators; break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _disarmed_pwm[i] = PWM_HIGHEST_MAX; - } else if (pwm->values[i] < PWM_LOWEST_MIN) { - _disarmed_pwm[i] = PWM_LOWEST_MIN; - } else { - _disarmed_pwm[i] = pwm->values[i]; + + 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; } - } - /* - * 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++; + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; + + } else { + _disarmed_pwm[i] = pwm->values[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; } - 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; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + + pwm->channel_count = _max_actuators; break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } 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 { - _min_pwm[i] = pwm->values[i]; + + 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++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } 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 { + _min_pwm[i] = pwm->values[i]; + } + } + + break; } - 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; - } + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - 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; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; break; } - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] < PWM_LOWEST_MAX) { - _max_pwm[i] = PWM_LOWEST_MAX; - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _max_pwm[i] = PWM_HIGHEST_MAX; - } else { - _max_pwm[i] = pwm->values[i]; + + 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++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; + + } else { + _max_pwm[i] = pwm->values[i]; + } } + + break; } - 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]; + 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; } - pwm->channel_count = _max_actuators; - arg = (unsigned long)&pwm; - break; - } case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): @@ -910,6 +946,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0): if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); + } else { ret = -EINVAL; } @@ -946,18 +983,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; - case PWM_SERVO_GET_COUNT: + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { case MODE_6PWM: *(unsigned *)arg = 6; break; + case MODE_4PWM: *(unsigned *)arg = 4; break; + case MODE_2PWM: *(unsigned *)arg = 2; break; + default: ret = -EINVAL; break; @@ -1015,6 +1055,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -1049,10 +1090,81 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) for (uint8_t i = 0; i < count; i++) { up_pwm_servo_set(i, values[i]); } + return count * 2; } void +PX4FMU::sensor_reset(int ms) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); + stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + + stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_GYRO_DRDY_OFF); + stm32_configgpio(GPIO_MAG_DRDY_OFF); + stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); +#endif +#endif +} + + +void PX4FMU::gpio_reset(void) { /* @@ -1062,6 +1174,7 @@ PX4FMU::gpio_reset(void) for (unsigned i = 0; i < _ngpio; i++) { if (_gpio_tab[i].input != 0) { stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { stm32_configgpio(_gpio_tab[i].output); } @@ -1078,6 +1191,7 @@ void PX4FMU::gpio_set_function(uint32_t gpios, int function) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -1089,6 +1203,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) if (GPIO_SET_OUTPUT == function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } + #endif /* configure selected GPIOs as required */ @@ -1113,9 +1228,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); + #endif } @@ -1154,6 +1271,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) gpio_reset(); break; + case GPIO_SENSOR_RAIL_RESET: + sensor_reset(arg); + break; + case GPIO_SET_OUTPUT: case GPIO_SET_INPUT: case GPIO_SET_ALT_1: @@ -1227,8 +1348,9 @@ fmu_new_mode(PortMode new_mode) #endif break; - /* mixed modes supported on v1 board only */ + /* mixed modes supported on v1 board only */ #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + case PORT_FULL_SERIAL: /* set all multi-GPIOs to serial mode */ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; @@ -1251,6 +1373,7 @@ fmu_new_mode(PortMode new_mode) servo_mode = PX4FMU::MODE_2PWM; break; #endif + default: return -1; } @@ -1305,14 +1428,30 @@ fmu_stop(void) } void +sensor_reset(int ms) +{ + int fd; + int ret; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) + err(1, "servo arm failed"); + +} + +void test(void) { int fd; - unsigned servo_count = 0; + unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; int ret; - + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) @@ -1320,9 +1459,9 @@ test(void) 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"); - } + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } warnx("Testing %u servos", (unsigned)servo_count); @@ -1335,32 +1474,38 @@ test(void) for (;;) { /* sweep all servos between 1000..2000 */ 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 == 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 < 2000) { pwm_value++; + } else { direction = -1; } + } else { if (pwm_value > 1000) { pwm_value--; + } else { direction = 1; } @@ -1372,6 +1517,7 @@ test(void) 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]); } @@ -1379,12 +1525,14 @@ test(void) /* 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; + break; } } } @@ -1457,6 +1605,7 @@ fmu_main(int argc, char *argv[]) new_mode = PORT_FULL_PWM; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + } else if (!strcmp(verb, "mode_serial")) { new_mode = PORT_FULL_SERIAL; @@ -1489,11 +1638,24 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "fake")) fake(argc - 1, argv + 1); + if (!strcmp(verb, "sensor_reset")) { + if (argc > 2) { + int reset_time = strtol(argv[2], 0, 0); + sensor_reset(reset_time); + + } else { + sensor_reset(0); + warnx("resettet default time"); + } + exit(0); + } + + fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm, test\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f..cbdd5acc4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -54,6 +54,7 @@ #include <unistd.h> #include <fcntl.h> #include <math.h> +#include <crc32.h> #include <arch/board/board.h> @@ -72,7 +73,6 @@ #include <systemlib/param/param.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/safety.h> @@ -95,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) +#define PX4IO_CHECK_CRC _IOC(0xff00, 3) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -226,30 +228,36 @@ private: device::Device *_interface; // XXX - unsigned _hardware; ///< Hardware revision - unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO - unsigned _max_controls; ///<Maximum # of controls supported by PX4IO - unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO - unsigned _max_relays; ///<Maximum relays supported by PX4IO - unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO + unsigned _hardware; ///< Hardware revision + unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO + unsigned _max_controls; ///< Maximum # of controls supported by PX4IO + unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO + unsigned _max_relays; ///< Maximum relays supported by PX4IO + unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values + unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels - volatile int _task; ///<worker task id - volatile bool _task_should_exit; ///<worker terminate flag + volatile int _task; ///< worker task id + volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///<mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. + int _thread_mavlink_fd; ///< mavlink file descriptor for thread. - perf_counter_t _perf_update; ///<local performance counter + perf_counter_t _perf_update; ///<local performance counter for status updates + perf_counter_t _perf_write; ///<local performance counter for PWM control writes + perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes /* cached IO state */ - uint16_t _status; ///<Various IO status flags - uint16_t _alarms; ///<Various IO alarms + uint16_t _status; ///< Various IO status flags + uint16_t _alarms; ///< Various IO alarms /* subscribed topics */ - int _t_actuators; ///< actuator controls topic + int _t_actuator_controls_0; ///< actuator controls group 0 topic + int _t_actuator_controls_1; ///< actuator controls group 1 topic + int _t_actuator_controls_2; ///< actuator controls group 2 topic + int _t_actuator_controls_3; ///< actuator controls group 3 topic int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic @@ -257,24 +265,22 @@ private: /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///<mixed outputs - actuator_controls_effective_s _controls_effective; ///<effective controls - bool _primary_pwm_device; ///<true if we are the default PWM output + bool _primary_pwm_device; ///< true if we are the default PWM output - float _battery_amp_per_volt; ///<current sensor amps/volt - float _battery_amp_bias; ///<current sensor bias - float _battery_mamphour_total;///<amp hours consumed so far - uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp + float _battery_amp_per_volt; ///< current sensor amps/volt + float _battery_amp_bias; ///< current sensor bias + float _battery_mamphour_total;///< amp hours consumed so far + uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power + bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power #endif /** @@ -288,9 +294,14 @@ private: void task_main(); /** - * Send controls to IO + * Send controls for one group to IO */ - int io_set_control_state(); + int io_set_control_state(unsigned group); + + /** + * Send all controls to IO + */ + int io_set_control_groups(); /** * Update IO's arming-related state @@ -328,11 +339,6 @@ private: int io_publish_raw_rc(); /** - * Fetch and publish the mixed control values. - */ - int io_publish_mixed_controls(); - - /** * Fetch and publish the PWM servo outputs. */ int io_publish_pwm_outputs(); @@ -459,20 +465,25 @@ PX4IO::PX4IO(device::Device *interface) : _max_transfer(16), /* sensible default */ _update_interval(0), _rc_handling_disabled(false), + _rc_chan_count(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), _thread_mavlink_fd(-1), - _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _perf_update(perf_alloc(PC_ELAPSED, "io update")), + _perf_write(perf_alloc(PC_ELAPSED, "io write")), + _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), _status(0), _alarms(0), - _t_actuators(-1), + _t_actuator_controls_0(-1), + _t_actuator_controls_1(-1), + _t_actuator_controls_2(-1), + _t_actuator_controls_3(-1), _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), _t_vehicle_command(-1), _to_input_rc(0), - _to_actuators_effective(0), _to_outputs(0), _to_battery(0), _to_servorail(0), @@ -769,15 +780,20 @@ PX4IO::task_main() * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - orb_set_interval(_t_actuators, 20); /* default to 50Hz */ + _t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0)); + orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */ + _t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1)); + orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */ + _t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2)); + orb_set_interval(_t_actuator_controls_2, 33); /* default to 30Hz */ + _t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3)); + orb_set_interval(_t_actuator_controls_3, 33); /* default to 30Hz */ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); _t_param = orb_subscribe(ORB_ID(parameter_update)); _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - if ((_t_actuators < 0) || + if ((_t_actuator_controls_0 < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || (_t_param < 0) || @@ -788,7 +804,7 @@ PX4IO::task_main() /* poll descriptor */ pollfd fds[1]; - fds[0].fd = _t_actuators; + fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; log("ready"); @@ -807,7 +823,11 @@ PX4IO::task_main() if (_update_interval > 100) _update_interval = 100; - orb_set_interval(_t_actuators, _update_interval); + orb_set_interval(_t_actuator_controls_0, _update_interval); + /* + * NOT changing the rate of groups 1-3 here, because only attitude + * really needs to run fast. + */ _update_interval = 0; } @@ -827,7 +847,10 @@ PX4IO::task_main() /* if we have new control data from the ORB, handle it */ if (fds[0].revents & POLLIN) { - io_set_control_state(); + + /* we're not nice to the lower-priority control groups and only check them + when the primary group updated (which is now). */ + (void)io_set_control_groups(); } if (now >= poll_last + IO_POLL_INTERVAL) { @@ -840,8 +863,7 @@ PX4IO::task_main() /* get raw R/C input from IO */ io_publish_raw_rc(); - /* fetch mixed servo controls and PWM outputs from IO */ - io_publish_mixed_controls(); + /* fetch PWM outputs from IO */ io_publish_pwm_outputs(); } @@ -871,7 +893,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { + if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { dsm_bind_ioctl((int)cmd.param2); } } @@ -901,7 +923,23 @@ PX4IO::task_main() /* re-upload RC input config as it may have changed */ io_set_rc_config(); + + /* re-set the battery scaling */ + int32_t voltage_scaling_val; + param_t voltage_scaling_param; + + /* set battery voltage scaling */ + param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); + + /* send scaling voltage to IO */ + uint16_t scaling = voltage_scaling_val; + int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); + + if (pret != OK) { + log("voltage scaling upload failed"); + } } + } perf_end(_perf_update); @@ -922,20 +960,74 @@ out: } int -PX4IO::io_set_control_state() +PX4IO::io_set_control_groups() +{ + int ret = io_set_control_state(0); + + /* send auxiliary control groups */ + (void)io_set_control_state(1); + (void)io_set_control_state(2); + (void)io_set_control_state(3); + + return ret; +} + +int +PX4IO::io_set_control_state(unsigned group) { actuator_controls_s controls; ///< actuator outputs uint16_t regs[_max_actuators]; /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + bool changed; + + switch (group) { + case 0: + { + orb_check(_t_actuator_controls_0, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); + } + } + break; + case 1: + { + orb_check(_t_actuator_controls_1, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls); + } + } + break; + case 2: + { + orb_check(_t_actuator_controls_2, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls); + } + } + break; + case 3: + { + orb_check(_t_actuator_controls_3, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls); + } + } + break; + } + + if (!changed) + return -1; for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); + return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); } @@ -1003,8 +1095,10 @@ PX4IO::io_set_rc_config() * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical * controls. */ + + /* fill the mapping with an error condition triggering value */ for (unsigned i = 0; i < _max_rc_input; i++) - input_map[i] = -1; + input_map[i] = UINT8_MAX; /* * NOTE: The indices for mapped channels are 1-based @@ -1031,11 +1125,10 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - ichan = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); - for (unsigned i = 0; i < _max_rc_input; i++) - if (input_map[i] == -1) - input_map[i] = ichan++; + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 4; /* * Iterate all possible RC inputs. @@ -1309,6 +1402,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = regs[0]; + if (channel_count != _rc_chan_count) + perf_count(_perf_chan_count); + + _rc_chan_count = channel_count; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1364,50 +1462,6 @@ PX4IO::io_publish_raw_rc() } int -PX4IO::io_publish_mixed_controls() -{ - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - - /* if not taking raw PPM from us, must be mixing */ - if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM) - return OK; - - /* data we are going to fetch */ - actuator_controls_effective_s controls_effective; - controls_effective.timestamp = hrt_absolute_time(); - - /* get actuator controls from IO */ - uint16_t act[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); - - if (ret != OK) - return ret; - - /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) - controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); - - /* laxily advertise on first publication */ - if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); - - } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); - } - - return OK; -} - -int PX4IO::io_publish_pwm_outputs() { /* if no FMU comms(!) just don't publish */ @@ -1659,11 +1713,13 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -1741,6 +1797,16 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); + + if (raw_inputs > 0) { + int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + printf("RC data (PPM frame len) %u us\n", frame_len); + + if ((frame_len - raw_inputs * 2000 - 3000) < 0) { + printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); + } + } + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); @@ -2146,6 +2212,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; + case PX4IO_REBOOT_BOOTLOADER: + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + return -EINVAL; + + /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); + // we don't expect a reply from this operation + ret = OK; + break; + + case PX4IO_CHECK_CRC: { + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + if (ret != OK) + return ret; + if (io_crc != arg) { + debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + break; + } + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2176,7 +2265,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) count = _max_actuators; if (count > 0) { + + perf_begin(_perf_write); int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + perf_end(_perf_write); if (ret != OK) return ret; @@ -2255,7 +2347,7 @@ void start(int argc, char *argv[]) { if (g_dev != nullptr) - errx(1, "already loaded"); + errx(0, "already loaded"); /* allocate the interface */ device::Device *interface = get_interface(); @@ -2530,7 +2622,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[4]; /* work out what we're uploading... */ if (argc > 2) { @@ -2588,6 +2680,39 @@ px4io_main(int argc, char *argv[]) if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + warnx("usage: px4io forceupdate MAGIC filename"); + exit(1); + } + if (g_dev == nullptr) { + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + } + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + /* commands below here require a started driver */ if (g_dev == nullptr) @@ -2673,6 +2798,49 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "checkcrc")) { + /* + check IO CRC against CRC of a file + */ + if (argc <= 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + int fd = open(argv[2], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[2], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -2690,5 +2858,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d01dedb0d..41f93a8ee 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,7 +274,10 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 1000); + // the small recv timeout here is to allow for fast + // drain when rebooting the io board for a forced + // update of the fw without using the safety switch + ret = recv(c, 40); #ifdef UDEBUG if (ret == OK) { diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a..b7c9b89a4 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -168,7 +168,7 @@ # error HRT_TIMER_CLOCK must be greater than 1MHz #endif -/* +/** * Minimum/maximum deadlines. * * These are suitable for use with a 16-bit timer/counter clocked @@ -276,12 +276,16 @@ static void hrt_call_invoke(void); * Specific registers and bits used by PPM sub-functions */ #ifdef HRT_PPM_CHANNEL -/* +/* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ # ifdef CONFIG_ARCH_CORTEXM3 +# undef GTIM_CCER_CC1NP +# undef GTIM_CCER_CC2NP +# undef GTIM_CCER_CC3NP +# undef GTIM_CCER_CC4NP # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 @@ -332,14 +336,21 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */ -# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2500 /* shortest valid start gap */ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ -#define PPM_MAX_CHANNELS 12 +#define PPM_MIN_CHANNELS 5 +#define PPM_MAX_CHANNELS 20 + +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ + __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; @@ -353,11 +364,12 @@ unsigned ppm_pulse_next; static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; -/* PPM decoder state machine */ +/** PPM decoder state machine */ struct { - uint16_t last_edge; /* last capture time */ - uint16_t last_mark; /* last significant edge */ - unsigned next_channel; + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ enum { UNSYNCH = 0, ARM, @@ -379,7 +391,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCER_PPM 0 #endif /* HRT_PPM_CHANNEL */ -/* +/** * Initialise the timer we are going to use. * * We expect that we'll own one of the reduced-function STM32 general @@ -425,7 +437,7 @@ hrt_tim_init(void) } #ifdef HRT_PPM_CHANNEL -/* +/** * Handle the PPM decoder state machine. */ static void @@ -440,9 +452,8 @@ hrt_ppm_decode(uint32_t status) if (status & SR_OVF_PPM) goto error; - /* how long since the last edge? */ + /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; - ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; @@ -455,14 +466,39 @@ hrt_ppm_decode(uint32_t status) */ if (width >= PPM_MIN_START) { - /* export the last set of samples if we got something sensible */ - if (ppm.next_channel > 4) { - for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) - ppm_buffer[i] = ppm_temp_buffer[i]; + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; - ppm_decoded_channels = i; - ppm_last_valid_decode = hrt_absolute_time(); + ppm_last_valid_decode = hrt_absolute_time(); + } } /* reset for the next frame */ @@ -471,29 +507,39 @@ hrt_ppm_decode(uint32_t status) /* next edge is the reference for the first channel */ ppm.phase = ARM; + ppm.last_edge = count; return; } switch (ppm.phase) { case UNSYNCH: /* we are waiting for a start pulse - nothing useful to do here */ - return; + break; case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ /* record the mark timing, expect an inactive edge */ - ppm.last_mark = count; - ppm.phase = INACTIVE; - return; + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; case INACTIVE: + + /* we expect a short pulse */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too short or too long */ + /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; - return; + break; case ACTIVE: /* determine the interval from the last mark */ @@ -514,10 +560,13 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + break; } + ppm.last_edge = count; + return; + /* the state machine is corrupted; reset it */ error: @@ -528,7 +577,7 @@ error: } #endif /* HRT_PPM_CHANNEL */ -/* +/** * Handle the compare interupt by calling the callout dispatcher * and then re-scheduling the next deadline. */ @@ -557,6 +606,7 @@ hrt_tim_isr(int irq, void *context) hrt_ppm_decode(status); } + #endif /* was this a timer tick? */ @@ -575,7 +625,7 @@ hrt_tim_isr(int irq, void *context) return OK; } -/* +/** * Fetch a never-wrapping absolute time value in microseconds from * some arbitrary epoch shortly after system start. */ @@ -622,7 +672,7 @@ hrt_absolute_time(void) return abstime; } -/* +/** * Convert a timespec to absolute time */ hrt_abstime @@ -636,7 +686,7 @@ ts_to_abstime(struct timespec *ts) return result; } -/* +/** * Convert absolute time to a timespec. */ void @@ -647,7 +697,7 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) ts->tv_nsec = abstime * 1000; } -/* +/** * Compare a time value with the current time. */ hrt_abstime @@ -662,7 +712,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then) return delta; } -/* +/** * Store the absolute time in an interrupt-safe fashion */ hrt_abstime @@ -677,7 +727,7 @@ hrt_store_absolute_time(volatile hrt_abstime *now) return ts; } -/* +/** * Initalise the high-resolution timing module. */ void @@ -692,7 +742,7 @@ hrt_init(void) #endif } -/* +/** * Call callout(arg) after interval has elapsed. */ void @@ -705,7 +755,7 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v arg); } -/* +/** * Call callout(arg) at calltime. */ void @@ -714,7 +764,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v hrt_call_internal(entry, calltime, 0, callout, arg); } -/* +/** * Call callout(arg) every period. */ void @@ -733,6 +783,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); @@ -746,7 +803,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqrestore(flags); } -/* +/** * If this returns true, the call has been invoked and removed from the callout list. * * Always returns false for repeating callouts. @@ -757,7 +814,7 @@ hrt_called(struct hrt_call *entry) return (entry->deadline == 0); } -/* +/** * Remove the entry from the callout list. */ void @@ -839,13 +896,19 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + hrt_call_enter(call); } } } -/* +/** * Reschedule the next timer interrupt. * * This routine must be called with interrupts disabled. @@ -906,5 +969,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 3125ce246..391e40ac1 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -61,7 +61,6 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/filtered_bottom_flow.h> #include <systemlib/systemlib.h> #include <systemlib/perf_counter.h> diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 2eb58abd6..b66d1dba5 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,39 +45,29 @@ #include <geo/geo.h> #include <ecl/ecl.h> #include <mathlib/mathlib.h> +#include <systemlib/err.h> ECL_PitchController::ECL_PitchController() : _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_ff(0.0f), + _integrator_max(0.0f), + _max_rate_pos(0.0f), + _max_rate_neg(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _bodyrate_setpoint(0.0f) { } -float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, - bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { - /* get the usual dt estimate */ - uint64_t dt_micros = ecl_elapsed_time(&_last_run); - _last_run = ecl_absolute_time(); - float dt = (float)dt_micros * 1e-6f; - - /* lock integral for long intervals */ - if (dt_micros > 500000) - lock_integrator = true; - float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; - - /* input conditioning */ - if (!isfinite(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; - } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -100,34 +90,78 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc } /* calculate the offset in the rate resulting from rolling */ + //xxx needs explanation and conversion to body angular rates or should be removed float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; if (inverted) turn_offset = -turn_offset; + /* Calculate the error */ float pitch_error = pitch_setpoint - pitch; - /* rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; /* add turn offset */ _rate_setpoint += turn_offset; - _rate_error = _rate_setpoint - pitch_rate; + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint; + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; + } - float ilimit_scaled = _integrator_max * scaler; + } - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + return _rate_setpoint; +} - float id = _rate_error * k_i_rate * dt * scaler; +float ECL_PitchController::control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian + + /* Transform estimation to body angular rates */ + float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _rate_error = _bodyrate_setpoint - pitch_bodyrate; + + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; - - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed +// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); +// warnx("roll: _last_output %.4f", (double)_last_output); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 1e6cec6a1..30a82a86a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal pitch PID controller. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> * * Acknowledgements: * @@ -51,13 +52,18 @@ #include <stdbool.h> #include <stdint.h> -class __EXPORT ECL_PitchController +class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); - float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, - bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); + + + float control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -67,21 +73,27 @@ public: void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; + + void set_k_ff(float k_ff) { + _k_ff = k_ff; } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; } + void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; } + void set_roll_ff(float roll_ff) { _roll_ff = roll_ff; } @@ -94,13 +106,17 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; float _tc; float _k_p; float _k_i; - float _k_d; + float _k_ff; float _integrator_max; float _max_rate_pos; float _max_rate_neg; @@ -109,7 +125,7 @@ private: float _integrator; float _rate_error; float _rate_setpoint; - float _max_deflection_rad; + float _bodyrate_setpoint; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 0b1ffa7a4..9e7d35f68 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,21 +45,46 @@ #include <geo/geo.h> #include <ecl/ecl.h> #include <mathlib/mathlib.h> +#include <systemlib/err.h> ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_ff(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _bodyrate_setpoint(0.0f) { +} + +float ECL_RollController::control_attitude(float roll_setpoint, float roll) +{ + + /* Calculate error */ + float roll_error = roll_setpoint - roll; + + /* Apply P controller */ + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + return _rate_setpoint; } -float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, - float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +float ECL_RollController::control_bodyrate(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); @@ -70,10 +95,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra if (dt_micros > 500000) lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; //xxx: param /* input conditioning */ +// warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (airspeed_min + airspeed_max); @@ -81,32 +107,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra airspeed = airspeed_min; } - float roll_error = roll_setpoint - roll; - _rate_setpoint = roll_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - _rate_error = _rate_setpoint - roll_rate; + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian + /* Transform estimation to body angular rates */ + float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian - float ilimit_scaled = _integrator_max * scaler; + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -115,11 +136,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0d4ea9333..92c64b95f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal roll PID controller. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> * * Acknowledgements: * @@ -51,13 +52,17 @@ #include <stdbool.h> #include <stdint.h> -class __EXPORT ECL_RollController +class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); - float control(float roll_setpoint, float roll, float roll_rate, - float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + float control_attitude(float roll_setpoint, float roll); + + float control_bodyrate(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -66,18 +71,23 @@ public: _tc = time_constant; } } + void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; + + void set_k_ff(float k_ff) { + _k_ff = k_ff; } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } @@ -90,19 +100,23 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; float _tc; float _k_p; float _k_i; - float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _last_output; float _integrator; float _rate_error; float _rate_setpoint; - float _max_deflection_rad; + float _bodyrate_setpoint; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index b786acf24..5e2200727 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,29 +44,125 @@ #include <geo/geo.h> #include <ecl/ecl.h> #include <mathlib/mathlib.h> +#include <systemlib/err.h> ECL_YawController::ECL_YawController() : _last_run(0), - _last_error(0.0f), + _k_p(0.0f), + _k_i(0.0f), + _k_ff(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), _last_output(0.0f), - _last_rate_hp_out(0.0f), - _last_rate_hp_in(0.0f), - _k_d_last(0.0f), - _integrator(0.0f) + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), + _coordinated_min_speed(1.0f) { +} + +float ECL_YawController::control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_v, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint) +{ +// static int counter = 0; + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = 0.0f; + if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; +// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); + } + +// if(counter % 20 == 0) { +// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch)); +// } + } + + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + +// counter++; + + if(!isfinite(_rate_setpoint)){ + warnx("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + + return _rate_setpoint; } -float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, - float airspeed_min, float airspeed_max, float aspeed) +float ECL_YawController::control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float pitch_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; + + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + + /* Transform estimation to body angular rates */ + float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * dt; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - return 0.0f; + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 66b227918..03f3202d0 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -35,6 +35,15 @@ * @file ecl_yaw_controller.h * Definition of a simple orthogonal coordinated turn yaw PID controller. * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. */ #ifndef ECL_YAW_CONTROLLER_H #define ECL_YAW_CONTROLLER_H @@ -42,47 +51,73 @@ #include <stdbool.h> #include <stdint.h> -class __EXPORT ECL_YawController +class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); - float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, - float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + float control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_v, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint); + + float control_bodyrate( float roll, float pitch, + float pitch_rate, float yaw_rate, + float pitch_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator); void reset_integrator(); - void set_k_side(float k_a) { - _k_side = k_a; + void set_k_p(float k_p) { + _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; - } - void set_k_roll_ff(float k_roll_ff) { - _k_roll_ff = k_roll_ff; + + void set_k_ff(float k_ff) { + _k_ff = k_ff; } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + + void set_coordinated_min_speed(float coordinated_min_speed) { + _coordinated_min_speed = coordinated_min_speed; + } + + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _rate_setpoint; + } + + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; - - float _k_side; + float _k_p; float _k_i; - float _k_d; - float _k_roll_ff; + float _k_ff; float _integrator_max; - - float _last_error; + float _max_rate; + float _roll_ff; float _last_output; - float _last_rate_hp_out; - float _last_rate_hp_in; - float _k_d_last; float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; + float _coordinated_min_speed; }; diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959..11def2371 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::max(wp_radius, _L1_distance); + return math::min(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons */ // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { _lateral_accel = lateral_accel_sp_center; _circle_mode = false; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d..5a56dce65 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -2,6 +2,7 @@ #include "tecs.h" #include <ecl/ecl.h> +#include <systemlib/err.h> using namespace math; @@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists // Use 50% of maximum energy rate to allow margin for total energy contgroller - float velRateMax; - float velRateMin; - - if ((_badDescent) || (_underspeed)) { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - - } else { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - } - - // Apply rate limit - if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; - _TAS_rate_dem = velRateMax; - - } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; - _TAS_rate_dem = velRateMin; - - } else { - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; - } +// float velRateMax; +// float velRateMin; +// +// if ((_badDescent) || (_underspeed)) { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// +// } else { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// } +// +// // Apply rate limit +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// _TAS_rate_dem = velRateMax; +// +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// _TAS_rate_dem = velRateMin; +// +// } else { +// _TAS_dem_adj = _TAS_dem; +// +// +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// } + + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now // Constrain speed demand again to protect against bad values on initialisation. _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); - _TAS_dem_last = _TAS_dem; +// _TAS_dem_last = _TAS_dem; + +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin); +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed); } -void TECS::_update_height_demand(float demand) +void TECS::_update_height_demand(float demand, float state) { - // Apply 2 point moving average to demanded height - // This is required because height demand is only updated at 5Hz - _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); - _hgt_dem_in_old = _hgt_dem; - - // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, - // _maxClimbRate); +// // Apply 2 point moving average to demanded height +// // This is required because height demand is only updated at 5Hz +// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); +// _hgt_dem_in_old = _hgt_dem; +// +// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, +// // _maxClimbRate); +// +// // Limit height rate of change +// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; +// +// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; +// } +// +// _hgt_dem_prev = _hgt_dem; +// +// // Apply first order lag to height demand +// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; +// _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; +// _hgt_dem_adj_last = _hgt_dem_adj; +// +// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, +// // _hgt_rate_dem); + + _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; + _hgt_dem_adj_last = _hgt_dem_adj; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change - if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + if (_hgt_rate_dem > _maxClimbRate) { + _hgt_rate_dem = _maxClimbRate; - } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } else if (_hgt_rate_dem < -_maxSinkRate) { + _hgt_rate_dem = -_maxSinkRate; } - _hgt_dem_prev = _hgt_dem; - - // Apply first order lag to height demand - _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; - _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; - _hgt_dem_adj_last = _hgt_dem_adj; - - // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, - // _hgt_rate_dem); + //warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) @@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); - STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); @@ -299,7 +324,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time - if (fabsf(_throttle_slewrate) < 0.01f) { + if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; _throttle_dem = constrain(_throttle_dem, @@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void) // 1) Underspeed protection not active // 2) Specific total energy error > 0 // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set - float STEdot = _SPEdot + _SKEdot; - - if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { - _badDescent = true; - - } else { - _badDescent = false; - } +// float STEdot = _SPEdot + _SKEdot; +// +// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { +// +// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem); +// _badDescent = true; +// +// } else { +// _badDescent = false; +// } + + _badDescent = false; } void TECS::_update_pitch(void) @@ -404,10 +433,18 @@ void TECS::_update_pitch(void) // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle + // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the + // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + if (_climbOutDem) + { + temp += _PITCHminf * gainInv; + } _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; @@ -500,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar _update_speed_demand(); // Calculate the height demand - _update_height_demand(hgt_dem); + _update_height_demand(hgt_dem, baro_altitude); // Detect underspeed condition _detect_underspeed(); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index f8f832ed7..4fc009da9 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -180,6 +180,14 @@ public: _indicated_airspeed_max = airspeed; } + void set_heightrate_p(float heightrate_p) { + _heightrate_p = heightrate_p; + } + + void set_speedrate_p(float speedrate_p) { + _speedrate_p = speedrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -203,6 +211,8 @@ private: float _vertAccLim; float _rollComp; float _spdWeight; + float _heightrate_p; + float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; @@ -329,7 +339,7 @@ private: void _update_speed_demand(void); // Update the demanded height - void _update_height_demand(float demand); + void _update_height_demand(float demand, float state); // Detect an underspeed condition void _detect_underspeed(void); diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba..f64bfb41a 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -387,6 +387,45 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d return return_value; } +__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) +{ + double current_x_rad = lat_next / 180.0 * M_PI; + double current_y_rad = lon_next / 180.0 * M_PI; + double x_rad = lat_now / 180.0 * M_PI; + double y_rad = lon_now / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + float dxy = CONSTANTS_RADIUS_OF_EARTH * c; + float dz = alt_now - alt_next; + + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + + return sqrtf(dxy * dxy + dz * dz); +} + + +__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) +{ + float dx = x_now - x_next; + float dy = y_now - y_next; + float dz = z_now - z_next; + + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); +} + __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ @@ -465,4 +504,26 @@ __EXPORT float _wrap_360(float bearing) return bearing; } +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = fence->count; + bool c = false; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && + (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / + (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) + c = !c; + return c; +} + + diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 123ff80f1..5f92e14cf 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -47,6 +47,9 @@ #pragma once +#include "uORB/topics/fence.h" +#include "uORB/topics/vehicle_global_position.h" + __BEGIN_DECLS #include <stdbool.h> @@ -121,9 +124,34 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); +/* + * Calculate distance in global frame + */ +__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); + +/* + * Calculate distance in local frame (NED) + */ +__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); + __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +/** + * Return whether craft is inside geofence. + * + * Calculate whether point is inside arbitrary polygon + * @param craft pointer craft coordinates + * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected + * @return true: craft is inside fence, false:craft is outside fence + */ +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence); + + __END_DECLS diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index efb17225d..3699d9bce 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -46,6 +46,10 @@ namespace math void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) { _cutoff_freq = cutoff_freq; + if (_cutoff_freq <= 0.0f) { + // no filtering + return; + } float fr = sample_freq/_cutoff_freq; float ohm = tanf(M_PI_F/fr); float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; @@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) float LowPassFilter2p::apply(float sample) { + if (_cutoff_freq <= 0.0f) { + // no filtering + return sample; + } // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 18fb6dcbc..ecca04dd7 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -41,6 +41,7 @@ #include "KalmanNav.hpp" #include <systemlib/err.h> +#include <geo/geo.h> // constants // Titterton pg. 52 @@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // attitude representations C_nb(), q(), - _accel_sub(-1), - _gyro_sub(-1), - _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), @@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), // parameters for ground station _vGyro(this, "V_GYRO"), _vAccel(this, "V_ACCEL"), @@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // gyro, accel and mag subscriptions - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - struct accel_report accel; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); - - struct mag_report mag; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); - // initialize rotation quaternion with a single raw sensor measurement - q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); // initialize dcm C_nb = Dcm(q); @@ -252,11 +248,21 @@ void KalmanNav::update() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + // XXX map_projection has internal global + // states that multiple things could change, + // should make map_projection take reference + // lat/lon and not have init + map_projection_init(lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS\n"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), - lat, lon, alt); + lat, lon, double(alt)); } // prediction step @@ -311,7 +317,7 @@ void KalmanNav::updatePublications() { using namespace math; - // position publication + // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; @@ -324,6 +330,31 @@ void KalmanNav::updatePublications() _pos.vz = vD; _pos.yaw = psi; + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = getLatDegE7(); + _localPos.ref_lon = getLonDegE7(); + _localPos.ref_alt = 0; + _localPos.landed = landed; + // attitude publication _att.timestamp = _pubTimeStamp; _att.roll = phi; @@ -347,8 +378,10 @@ void KalmanNav::updatePublications() // selectively update publications, // do NOT call superblock do-all method - if (_positionInitialized) + if (_positionInitialized) { _pos.update(); + _localPos.update(); + } if (_attitudeInitialized) _att.update(); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 799fc2fb9..a69bde1a6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/parameter_update.h> @@ -142,12 +143,8 @@ protected: control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ // publications control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ - - int _accel_sub; /**< Accelerometer subscription */ - int _gyro_sub; /**< Gyroscope subscription */ - int _mag_sub; /**< Magnetometer subscription */ - // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -164,8 +161,10 @@ protected: float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ double lat, lon; /**< lat, lon radians */ - float alt; /**< altitude, meters */ // parameters + float alt; /**< altitude, meters */ + double lat0, lon0; /**< reference latitude and longitude */ + float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 52dac652b..3cfddf28e 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -52,11 +52,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); -/* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); +/* accel measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); +/* mag measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); +/* offset estimation - UNUSED */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); @@ -72,10 +74,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q3 = param_find("EKF_ATT_V3_Q3"); h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V3_R0"); - h->r1 = param_find("EKF_ATT_V3_R1"); - h->r2 = param_find("EKF_ATT_V3_R2"); - h->r3 = param_find("EKF_ATT_V3_R3"); + h->r0 = param_find("EKF_ATT_V4_R0"); + h->r1 = param_find("EKF_ATT_V4_R1"); + h->r2 = param_find("EKF_ATT_V4_R2"); + h->r3 = param_find("EKF_ATT_V4_R3"); h->roll_off = param_find("ATT_ROLL_OFF3"); h->pitch_off = param_find("ATT_PITCH_OFF3"); diff --git a/src/modules/attitude_estimator_so3/README b/src/modules/attitude_estimator_so3/README new file mode 100644 index 000000000..02ab66ee4 --- /dev/null +++ b/src/modules/attitude_estimator_so3/README @@ -0,0 +1,3 @@ +Synopsis + + nsh> attitude_estimator_so3_comp start
\ No newline at end of file diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 86bda3c75..e79726613 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,16 +1,49 @@ -/* - * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim <limhyon@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: * - * @file attitude_estimator_so3_comp_main.c + * 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 attitude_estimator_so3_main.cpp * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * + * * Theory of nonlinear complementary filters on the SO(3) is based on [1]. * Quaternion realization of [1] is based on [2]. * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * + * * References * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 @@ -46,94 +79,91 @@ #ifdef __cplusplus extern "C" { #endif -#include "attitude_estimator_so3_comp_params.h" +#include "attitude_estimator_so3_params.h" #ifdef __cplusplus } #endif -extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ +static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */ + +//! Auxiliary variables to reduce number of repeated operations static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static bool bFilterInit = false; - -//! Auxiliary variables to reduce number of repeated operations static float q0q0, q0q1, q0q2, q0q3; static float q1q1, q1q2, q1q3; static float q2q2, q2q3; static float q3q3; - -//! Serial packet related -static int uart; -static int baudrate; +static bool bFilterInit = false; /** - * Mainloop of attitude_estimator_so3_comp. + * Mainloop of attitude_estimator_so3. */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); +int attitude_estimator_so3_thread_main(int argc, char *argv[]); /** * Print the correct usage. */ static void usage(const char *reason); +/* Function prototypes */ +float invSqrt(float number); +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n" - "-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n" - "ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n"); + fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n"); exit(1); } /** - * The attitude_estimator_so3_comp app only briefly exists to start + * The attitude_estimator_so3 app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. * * The actual stack size should be set in the call * to task_create(). */ -int attitude_estimator_so3_comp_main(int argc, char *argv[]) +int attitude_estimator_so3_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); - - if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("attitude_estimator_so3_comp already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", + attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12400, - attitude_estimator_so3_comp_thread_main, - (const char **)argv); + 14000, + attitude_estimator_so3_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - while(thread_running){ + while (thread_running){ usleep(200000); - printf("."); } - printf("terminated."); + + warnx("stopped"); exit(0); } @@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) //--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) { +float invSqrt(float number) +{ volatile long i; volatile float x, y; volatile const float f = 1.5F; @@ -221,48 +252,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa q3q3 = q3 * q3; } -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) +{ float recipNorm; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - //! Make filter converge to initial solution faster - //! This function assumes you are in static position. - //! WARNING : in case air reboot, this can cause problem. But this is very - //! unlikely happen. - if(bFilterInit == false) - { + // Make filter converge to initial solution faster + // This function assumes you are in static position. + // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. + if(bFilterInit == false) { NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); bFilterInit = true; } //! If magnetometer measurement is available, use it. - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { float hx, hy, hz, bx, bz; float halfwx, halfwy, halfwz; // Normalise magnetometer measurement // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); + bx = sqrt(hx * hx + hy * hy); + bz = hz; - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); } // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) @@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[1] += twoKi * halfey * dt; gyro_bias[2] += twoKi * halfez * dt; - gx += gyro_bias[0]; // apply integral feedback + + // apply integral feedback + gx += gyro_bias[0]; gy += gyro_bias[1]; gz += gyro_bias[2]; } @@ -337,208 +369,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl q3 *= recipNorm; // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void send_uart_byte(char c) -{ - write(uart,&c,1); -} - -void send_uart_bytes(uint8_t *data, int length) -{ - write(uart,data,(size_t)(sizeof(uint8_t)*length)); -} - -void send_uart_float(float f) { - uint8_t * b = (uint8_t *) &f; - - //! Assume float is 4-bytes - for(int i=0; i<4; i++) { - - uint8_t b1 = (b[i] >> 4) & 0x0f; - uint8_t b2 = (b[i] & 0x0f); - - uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; - uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; - - send_uart_bytes(&c1,1); - send_uart_bytes(&c2,1); - } -} - -void send_uart_float_arr(float *arr, int length) -{ - for(int i=0;i<length;++i) - { - send_uart_float(arr[i]); - send_uart_byte(','); - } -} - -int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - int speed; - - switch (baud) { - case 0: speed = B0; break; - case 50: speed = B50; break; - case 75: speed = B75; break; - case 110: speed = B110; break; - case 134: speed = B134; break; - case 150: speed = B150; break; - case 200: speed = B200; break; - case 300: speed = B300; break; - case 600: speed = B600; break; - case 1200: speed = B1200; break; - case 1800: speed = B1800; break; - case 2400: speed = B2400; break; - case 4800: speed = B4800; break; - case 9600: speed = B9600; break; - case 19200: speed = B19200; break; - case 38400: speed = B38400; break; - case 57600: speed = B57600; break; - case 115200: speed = B115200; break; - case 230400: speed = B230400; break; - case 460800: speed = B460800; break; - case 921600: speed = B921600; break; - default: - printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); - return -EINVAL; - } - - printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(uart); - return -1; - } - - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; - } - - return uart; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; } /* - * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - -/* - * EKF Attitude Estimator main function. + * Nonliner complementary filter on SO(3), attitude estimator main function. * - * Estimates the attitude recursively once started. + * Estimates the attitude once started. * * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) +int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - //! Serial debug related - int ch; - struct termios uart_config_original; - bool usb_uart; - bool debug_mode = false; - char *device_name = "/dev/ttyS2"; - baudrate = 115200; + const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds //! Time constant float dt = 0.005f; /* output euler angles */ float euler[3] = {0.0f, 0.0f, 0.0f}; - - float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - + + /* Initialization */ + float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ float acc[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f}; float mag[3] = {0.0f, 0.0f, 0.0f}; - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - //! -d <device_name>, default : /dev/ttyS2 - //! -b <baud_rate>, default : 115200 - while ((ch = getopt(argc,argv,"d:b:")) != EOF){ - switch(ch){ - case 'b': - baudrate = strtoul(optarg, NULL, 10); - if(baudrate == 0) - printf("invalid baud rate '%s'",optarg); - break; - case 'd': - device_name = optarg; - debug_mode = true; - break; - default: - usage("invalid argument"); - } - } - - if(debug_mode){ - printf("Opening debugging port for 3D visualization\n"); - uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) - printf("could not open %s", device_name); - else - printf("Open port success\n"); - } - - // print text - printf("Nonlinear SO3 Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); + warnx("main thread started"); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); @@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t att_pub = -1; + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; int printcounter = 0; thread_running = true; - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs @@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_so3_comp_params so3_comp_params; - struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; + struct attitude_estimator_so3_params so3_comp_params; + struct attitude_estimator_so3_param_handles so3_comp_param_handles; /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); + parameters_update(&so3_comp_param_handles, &so3_comp_params); uint64_t start_time = hrt_absolute_time(); bool initialized = false; + bool state_initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); + perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3"); /* Main loop*/ while (!thread_should_exit) { @@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - fprintf(stderr, - "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); + warnx("WARNING: Not getting sensors - sensor app running?"); } - } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds gyro_offsets[2] += raw.gyro_rad_s[2]; offset_count++; - if (hrt_absolute_time() - start_time > 3000000LL) { + if (hrt_absolute_time() > start_time + 3000000l) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; + warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); } } else { @@ -668,9 +533,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[0] = raw.timestamp; } - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { @@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds mag[1] = raw.magnetometer_ga[1]; mag[2] = raw.magnetometer_ga[2]; - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - static bool const_initialized = false; - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&so3_comp_param_handles, &so3_comp_params); - const_initialized = true; + if (!state_initialized && dt < 0.05f && dt > 0.001f) { + state_initialized = true; + warnx("state initialized"); } /* do not execute the filter if not initialized */ - if (!const_initialized) { + if (!state_initialized) { continue; } @@ -728,18 +576,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); + NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], + -acc[0], -acc[1], -acc[2], + mag[0], mag[1], mag[2], + so3_comp_params.Kp, + so3_comp_params.Ki, + dt); // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 + Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 + Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 + Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 + Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 + Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 + Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 + Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 + Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 //1-2-3 Representation. //Equation (290) @@ -747,29 +600,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw + euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - /* Do something */ + // Publish only finite euler angles + att.roll = euler[0] - so3_comp_params.roll_off; + att.pitch = euler[1] - so3_comp_params.pitch_off; + att.yaw = euler[2] - so3_comp_params.yaw_off; } else { /* due to inputs or numerical failure the output is invalid, skip it */ + // Due to inputs or numerical failure the output is invalid + warnx("infinite euler angles, rotation matrix:"); + warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); + warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); + warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + // Don't publish anything continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp > last_data + 12000) { + warnx("sensor data missed"); + } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; + + // Quaternion + att.q[0] = q0; + att.q[1] = q1; + att.q[2] = q2; + att.q[3] = q3; + att.q_valid = true; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - - //! Euler angle rate. But it needs to be investigated again. + // Euler angle rate. But it needs to be investigated again. /* att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); @@ -783,53 +649,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = 0; att.yawacc = 0; - //! Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - /* TODO: Bias estimation required */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - + + // Publish + if (att_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); + orb_advertise(ORB_ID(vehicle_attitude), &att); } perf_end(so3_comp_loop_perf); - - //! This will print out debug packet to visualization software - if(debug_mode) - { - float quat[4]; - quat[0] = q0; - quat[1] = q1; - quat[2] = q2; - quat[3] = q3; - send_uart_float_arr(quat,4); - send_uart_byte('\n'); - } } } } loopcounter++; - }// while + } thread_running = false; - /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); - return 0; } diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c new file mode 100755 index 000000000..0c8d522b4 --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim <limhyon@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> + * + * 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 attitude_estimator_so3_params.c + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include "attitude_estimator_so3_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_so3_param_handles *h) +{ + /* Filter gain parameters */ + h->Kp = param_find("SO3_COMP_KP"); + h->Ki = param_find("SO3_COMP_KI"); + + /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ + h->roll_off = param_find("SO3_ROLL_OFFS"); + h->pitch_off = param_find("SO3_PITCH_OFFS"); + h->yaw_off = param_find("SO3_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p) +{ + /* Update filter gain */ + param_get(h->Kp, &(p->Kp)); + param_get(h->Ki, &(p->Ki)); + + /* Update attitude offset */ + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h new file mode 100755 index 000000000..dfb4cad05 --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim <limhyon@gmail.com> + * Anton Babushkin <anton.babushkin@me.com> + * + * 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 attitude_estimator_so3_params.h + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include <systemlib/param/param.h> + +struct attitude_estimator_so3_params { + float Kp; + float Ki; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_so3_param_handles { + param_t Kp, Ki; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_so3_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p); diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk new file mode 100644 index 000000000..e29bb16a6 --- /dev/null +++ b/src/modules/attitude_estimator_so3/module.mk @@ -0,0 +1,8 @@ +# +# Attitude estimator (Nonlinear SO(3) complementary Filter) +# + +MODULE_COMMAND = attitude_estimator_so3 + +SRCS = attitude_estimator_so3_main.cpp \ + attitude_estimator_so3_params.c diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README deleted file mode 100644 index 79c50a531..000000000 --- a/src/modules/attitude_estimator_so3_comp/README +++ /dev/null @@ -1,5 +0,0 @@ -Synopsis - - nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200 - -Option -d is for debugging packet. See code for detailed packet structure. diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c deleted file mode 100755 index f962515df..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> - * - * @file attitude_estimator_so3_comp_params.c - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include "attitude_estimator_so3_comp_params.h" - -/* This is filter gain for nonlinear SO3 complementary filter */ -/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. - Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. - If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which - will compensate gyro bias which depends on temperature and vibration of your vehicle */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. - //! You can set this gain higher if you want more fast response. - //! But note that higher gain will give you also higher overshoot. -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) - //! This gain is depend on your vehicle status. - -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); - -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h) -{ - /* Filter gain parameters */ - h->Kp = param_find("SO3_COMP_KP"); - h->Ki = param_find("SO3_COMP_KI"); - - /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p) -{ - /* Update filter gain */ - param_get(h->Kp, &(p->Kp)); - param_get(h->Ki, &(p->Ki)); - - /* Update attitude offset */ - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - - return OK; -} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h deleted file mode 100755 index f00695630..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> - * - * @file attitude_estimator_so3_comp_params.h - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include <systemlib/param/param.h> - -struct attitude_estimator_so3_comp_params { - float Kp; - float Ki; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_so3_comp_param_handles { - param_t Kp, Ki; - param_t roll_off, pitch_off, yaw_off; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p); diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk deleted file mode 100644 index 92f43d920..000000000 --- a/src/modules/attitude_estimator_so3_comp/module.mk +++ /dev/null @@ -1,8 +0,0 @@ -# -# Attitude estimator (Nonlinear SO3 complementary Filter) -# - -MODULE_COMMAND = attitude_estimator_so3_comp - -SRCS = attitude_estimator_so3_comp_main.cpp \ - attitude_estimator_so3_comp_params.c diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb78..199bfb0da 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -69,7 +69,6 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_armed.h> @@ -194,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -215,8 +214,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -346,13 +343,12 @@ void print_status() warnx("arming: %s", armed_str); } -static orb_advert_t control_mode_pub; static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* only handle high-priority commands here */ @@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_NAV_TAKEOFF: { - if (armed->armed) { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } - - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - } else { - /* reject TAKEOFF not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -509,29 +482,37 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - default: - break; - } + /* Flight termination */ + case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); + if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + result = VEHICLE_CMD_RESULT_ACCEPTED; - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* we do not care in the high prio loop about commands we don't know */ - } else { - tune_negative(); + } else { + /* reject parachute depoyment not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); + } + break; - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case VEHICLE_CMD_PREFLIGHT_STORAGE: + /* ignore commands that handled in low prio loop */ + break; - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + default: + /* warn about unsupported commands */ + answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; + } - } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); } /* send any requested ACKs */ @@ -549,9 +530,6 @@ static struct actuator_armed_s armed; static struct safety_s safety; -/* flags for control apps */ -struct vehicle_control_mode_s control_mode; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -594,11 +572,9 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_DIRECT; + status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -610,9 +586,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status.offboard_control_signal_lost = true; - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; @@ -620,9 +593,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - // XXX just disable offboard control for now - control_mode.flag_control_offboard_enabled = false; - /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ @@ -634,8 +604,6 @@ int commander_thread_main(int argc, char *argv[]) armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -788,11 +756,9 @@ int commander_thread_main(int argc, char *argv[]) status.system_type == VEHICLE_TYPE_QUADROTOR || status.system_type == VEHICLE_TYPE_HEXAROTOR || status.system_type == VEHICLE_TYPE_OCTOROTOR) { - control_mode.flag_external_manual_override_ok = false; status.is_rotary_wing = true; } else { - control_mode.flag_external_manual_override_ok = true; status.is_rotary_wing = false; } @@ -844,7 +810,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX this would be the right approach to do it, but do we *WANT* this? // /* disarm if safety is now on and still armed */ // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); // } } @@ -871,7 +837,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); - if (status.condition_local_altitude_valid) { + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -975,10 +941,10 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -997,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1033,23 +999,17 @@ int commander_thread_main(int argc, char *argv[]) if (!home_position_set && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed + && global_position.valid) { /* copy position data to uORB home message, store it locally as well */ - // TODO use global position estimate - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; + home.lat = (double)global_position.lat / 1e7d; + home.lon = (double)global_position.lon / 1e7d; + home.altitude = (float)global_position.alt; - double home_lat_d = home.lat * 1e-7; - double home_lon_d = home.lon * 1e-7; - warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); /* announce new home position */ if (home_pub > 0) { @@ -1093,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1123,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1174,6 +1132,16 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ + if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); + if (flighttermination_res == TRANSITION_CHANGED) { + tune_positive(); + } + } else { + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1183,31 +1151,18 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - - /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); - - if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + handle_command(&status, &safety, &cmd, &armed); } /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool navigation_state_changed = check_navigation_state_changed(); + bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (navigation_state_changed || arming_state_changed) { - control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - } - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + if (arming_state_changed || main_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); status_changed = true; } @@ -1215,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[]) if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1529,132 +1482,6 @@ print_reject_arm(const char *msg) } } -transition_result_t -check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) -{ - transition_result_t res = TRANSITION_DENIED; - - if (status->main_state == MAIN_STATE_AUTO) { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - // TODO AUTO_LAND handling - if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { - return TRANSITION_NOT_CHANGED; - } - } - - if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { - /* possibly on ground, switch to TAKEOFF if needed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - return res; - } - } - - /* switch to AUTO mode */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } - - } else { - /* switch to MISSION when no RC control and first time in some AUTO mode */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } - } - - } else { - /* disarmed, always switch to AUTO_READY */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - - } else { - /* manual control modes */ - if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { - /* switch to failsafe mode */ - bool manual_control_old = control_mode->flag_control_manual_enabled; - - if (!status->condition_landed) { - /* in air: try to hold position */ - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - - } else { - /* landed: don't try to hold position but land (if taking off) */ - res = TRANSITION_DENIED; - } - - if (res == TRANSITION_DENIED) { - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - } - - control_mode->flag_control_manual_enabled = false; - - if (res == TRANSITION_NOT_CHANGED && manual_control_old) { - /* mark navigation state as changed to force immediate flag publishing */ - set_navigation_state_changed(); - res = TRANSITION_CHANGED; - } - - if (res == TRANSITION_CHANGED) { - if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); - - } else { - if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); - - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); - } - } - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - default: - break; - } - } - } - - return res; -} - void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { @@ -1724,7 +1551,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) continue; /* only handle low-priority commands here */ @@ -1762,7 +1590,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1802,7 +1630,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); break; } @@ -1854,7 +1682,7 @@ void *commander_low_prio_loop(void *arg) } default: - answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't answer on unsupported commands, it will be done in main loop */ break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..731e0e3ff 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,7 +46,6 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_control_mode.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -64,11 +63,10 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool navigation_state_changed = true; +static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* @@ -85,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { /* enforce lockdown in HIL */ - if (control_mode->flag_system_hil_enabled) { + if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; } else { armed->lockdown = false; @@ -108,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT || status->arming_state == ARMING_STATE_ARMED - || control_mode->flag_system_hil_enabled) { + || status->hil_state == HIL_STATE_ON) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -125,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -239,8 +237,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid) { + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -287,162 +286,11 @@ check_main_state_changed() } } -transition_result_t -navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == status->navigation_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_navigation_state) { - case NAVIGATION_STATE_DIRECT: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_STABILIZE: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_ALTHOLD: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_VECTOR: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_READY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LOITER: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_RTL: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LAND: - - /* deny transitions from landed state */ - if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->navigation_state = new_navigation_state; - control_mode->auto_state = status->navigation_state; - navigation_state_changed = true; - } - } - - return ret; -} - bool -check_navigation_state_changed() +check_flighttermination_state_changed() { - if (navigation_state_changed) { - navigation_state_changed = false; + if (flighttermination_state_changed) { + flighttermination_state_changed = false; return true; } else { @@ -450,16 +298,10 @@ check_navigation_state_changed() } } -void -set_navigation_state_changed() -{ - navigation_state_changed = true; -} - /** * Transition from one hil state to another */ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -488,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -507,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - current_control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); - // XXX also set lockdown here ret = OK; @@ -522,6 +360,45 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s } +/** +* Transition from one flightermination state to another +*/ +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->flighttermination_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; +} + + // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0bfdf36a8..e569fb4f3 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -48,7 +48,6 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/safety.h> -#include <uORB/topics/vehicle_control_mode.h> typedef enum { TRANSITION_DENIED = -1, @@ -58,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,12 +67,14 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); bool check_navigation_state_changed(); +bool check_flighttermination_state_changed(); + void set_navigation_state_changed(); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 448a42a99..e213ac17f 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + missionCmd.lat, + missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + lastMissionCmd.lat, + lastMissionCmd.lon, + missionCmd.lat, + missionCmd.lon); _psiCmd = _wrap_2pi(psiTrack - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 46dc1bec2..8cc0d77d4 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; + UOrbSubscription<mission_item_triplet_s> _missionCmd; UOrbSubscription<manual_control_setpoint_s> _manual; UOrbSubscription<vehicle_status_s> _status; UOrbSubscription<parameter_update_s> _param_update; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c new file mode 100644 index 000000000..dc2d6c312 --- /dev/null +++ b/src/modules/dataman/dataman.c @@ -0,0 +1,741 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Jean Cyr + * + * 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 dataman.c + * DATAMANAGER driver. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <sys/ioctl.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <queue.h> + +#include "dataman.h" + +/** + * data manager app start / stop handling function + * + * @ingroup apps + */ + +__EXPORT int dataman_main(int argc, char *argv[]); +__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); +__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); +__EXPORT int dm_clear(dm_item_t item); +__EXPORT int dm_restart(dm_reset_reason restart_type); + +/* Types of function calls supported by the worker task */ +typedef enum { + dm_write_func = 0, + dm_read_func, + dm_clear_func, + dm_restart_func, + dm_number_of_funcs +} dm_function_t; + +/* Work task work item */ +typedef struct { + sq_entry_t link; /**< list linkage */ + sem_t wait_sem; + dm_function_t func; + ssize_t result; + union { + struct { + dm_item_t item; + unsigned char index; + dm_persitence_t persistence; + const void *buf; + size_t count; + } write_params; + struct { + dm_item_t item; + unsigned char index; + void *buf; + size_t count; + } read_params; + struct { + dm_item_t item; + } clear_params; + struct { + dm_reset_reason reason; + } restart_params; + }; +} work_q_item_t; + +/* Usage statistics */ +static unsigned g_func_counts[dm_number_of_funcs]; + +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, + DM_KEY_WAYPOINTS_ONBOARD_MAX +}; + +/* Table of offset for index 0 of each item type */ +static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; + +/* The data manager store file handle and file name */ +static int g_fd = -1, g_task_fd = -1; +static const char *k_data_manager_device_path = "/fs/microsd/dataman"; + +/* The data manager work queues */ + +typedef struct { + sq_queue_t q; + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; + unsigned max_size; +} work_q_t; + +static work_q_t g_free_q; +static work_q_t g_work_q; + +sem_t g_work_queued_sema; +sem_t g_init_sema; + +static bool g_task_should_exit; /**< if true, dataman task should exit */ + +#define DM_SECTOR_HDR_SIZE 4 +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; + +static void init_q(work_q_t *q) +{ + sq_init(&(q->q)); + sem_init(&(q->mutex), 1, 1); + q->size = q->max_size = 0; +} + +static void destroy_q(work_q_t *q) +{ + sem_destroy(&(q->mutex)); +} + +static inline void +lock_queue(work_q_t *q) +{ + sem_wait(&(q->mutex)); +} + +static inline void +unlock_queue(work_q_t *q) +{ + sem_post(&(q->mutex)); +} + +static work_q_item_t * +create_work_item(void) +{ + work_q_item_t *item; + + lock_queue(&g_free_q); + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) + g_free_q.size--; + unlock_queue(&g_free_q); + + if (item == NULL) + item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + + if (item) + sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + + return item; +} + +/* Work queue management functions */ +static void +enqueue_work_item(work_q_item_t *item) +{ + /* put the work item on the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); +} + +static void +destroy_work_item(work_q_item_t *item) +{ + sem_destroy(&item->wait_sem); + lock_queue(&g_free_q); + sq_addfirst(&item->link, &(g_free_q.q)); + + if (++g_free_q.size > g_free_q.max_size) + g_free_q.max_size = g_free_q.size; + + unlock_queue(&g_free_q); +} + +static work_q_item_t * +dequeue_work_item(void) +{ + work_q_item_t *work; + lock_queue(&g_work_q); + + if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) + g_work_q.size--; + + unlock_queue(&g_work_q); + return work; +} + +/* Calculate the offset in file of specific item */ +static int +calculate_offset(dm_item_t item, unsigned char index) +{ + + /* Make sure the item type is valid */ + if (item >= DM_KEY_NUM_KEYS) + return -1; + + /* Make sure the index for this item type is valid */ + if (index >= g_per_item_max_index[item]) + return -1; + + /* Calculate and return the item index based on type and index */ + return g_key_offsets[item] + (index * k_sector_size); +} + +/* Each data item is stored as follows + * + * byte 0: Length of user data item + * byte 1: Persistence of this data item + * byte DM_SECTOR_HDR_SIZE... : data item value + * + * The total size must not exceed k_sector_size + */ + +/* write to the data manager file */ +static ssize_t +_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) +{ + unsigned char buffer[k_sector_size]; + size_t len; + int offset; + + /* Get the offset for this item */ + offset = calculate_offset(item, index); + + if (offset < 0) + return -1; + + /* Make sure caller has not given us more data than we can handle */ + if (count > DM_MAX_DATA_SIZE) + return -1; + + /* Write out the data, prefixed with length and persistence level */ + buffer[0] = count; + buffer[1] = persistence; + buffer[2] = 0; + buffer[3] = 0; + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + count += DM_SECTOR_HDR_SIZE; + + len = -1; + + if (lseek(g_task_fd, offset, SEEK_SET) == offset) + if ((len = write(g_task_fd, buffer, count)) == count) + fsync(g_task_fd); + + if (len != count) + return -1; + + /* All is well... return the number of user data written */ + return count - DM_SECTOR_HDR_SIZE; +} + +/* Retrieve from the data manager file */ +static ssize_t +_read(dm_item_t item, unsigned char index, void *buf, size_t count) +{ + unsigned char buffer[k_sector_size]; + int len, offset; + + /* Get the offset for this item */ + offset = calculate_offset(item, index); + + if (offset < 0) + return -1; + + /* Make sure the caller hasn't asked for more data than we can handle */ + if (count > DM_MAX_DATA_SIZE) + return -1; + + /* Read the prefix and data */ + len = -1; + if (lseek(g_task_fd, offset, SEEK_SET) == offset) + len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); + + /* Check for length issues */ + if (len < 0) + return -1; + + if (len == 0) + buffer[0] = 0; + + if (buffer[0] > 0) { + if (buffer[0] > count) + return -1; + + /* Looks good, copy it to the caller's buffer */ + memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + } + + /* Return the number of bytes of caller data read */ + return buffer[0]; +} + +static int +_clear(dm_item_t item) +{ + int i, result = 0; + + int offset = calculate_offset(item, 0); + + if (offset < 0) + return -1; + + for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + char buf[1]; + + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + if (read(g_task_fd, buf, 1) < 1) + break; + + if (buf[0]) { + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + buf[0] = 0; + + if (write(g_task_fd, buf, 1) != 1) { + result = -1; + break; + } + } + + offset += k_sector_size; + } + + fsync(g_task_fd); + return result; +} + +/* Tell the data manager about the type of the last reset */ +static int +_restart(dm_reset_reason reason) +{ + unsigned char buffer[2]; + int offset, result = 0; + + /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ + + /* Loop through all of the data segments and delete those that are not persistent */ + offset = 0; + + while (1) { + size_t len; + + /* Get data segment at current offset */ + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + len = read(g_task_fd, buffer, sizeof(buffer)); + + if (len == 0) + break; + + /* check if segment contains data */ + if (buffer[0]) { + int clear_entry = 0; + + /* Whether data gets deleted depends on reset type and data segment's persistence setting */ + if (reason == DM_INIT_REASON_POWER_ON) { + if (buffer[1] != DM_PERSIST_POWER_ON_RESET) { + clear_entry = 1; + } + + } else { + if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) { + clear_entry = 1; + } + } + + /* Set segment to unused if data does not persist */ + if (clear_entry) { + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + buffer[0] = 0; + + len = write(g_task_fd, buffer, 1); + + if (len != 1) { + result = -1; + break; + } + } + } + + offset += k_sector_size; + } + + fsync(g_task_fd); + + /* tell the caller how it went */ + return result; +} + +/* write to the data manager file */ +__EXPORT ssize_t +dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_write_func; + work->write_params.item = item; + work->write_params.index = index; + work->write_params.persistence = persistence; + work->write_params.buf = buf; + work->write_params.count = count; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + ssize_t result = work->result; + destroy_work_item(work); + return result; +} + +/* Retrieve from the data manager file */ +__EXPORT ssize_t +dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_read_func; + work->read_params.item = item; + work->read_params.index = index; + work->read_params.buf = buf; + work->read_params.count = count; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + ssize_t result = work->result; + destroy_work_item(work); + return result; +} + +__EXPORT int +dm_clear(dm_item_t item) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_clear_func; + work->clear_params.item = item; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + int result = work->result; + destroy_work_item(work); + return result; +} + +/* Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart(dm_reset_reason reason) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_restart_func; + work->restart_params.reason = reason; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + int result = work->result; + destroy_work_item(work); + return result; +} + +static int +task_main(int argc, char *argv[]) +{ + work_q_item_t *work; + + /* inform about start */ + warnx("Initializing.."); + + /* Initialize global variables */ + g_key_offsets[0] = 0; + + for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size); + + unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size); + + for (unsigned i = 0; i < dm_number_of_funcs; i++) + g_func_counts[i] = 0; + + g_task_should_exit = false; + + init_q(&g_work_q); + init_q(&g_free_q); + + sem_init(&g_work_queued_sema, 1, 0); + + g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); + if (g_task_fd < 0) { + warnx("Could not open data manager file %s", k_data_manager_device_path); + sem_post(&g_init_sema); + return -1; + } + if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { + close(g_task_fd); + warnx("Could not seek data manager file %s", k_data_manager_device_path); + sem_post(&g_init_sema); + return -1; + } + fsync(g_task_fd); + + g_fd = g_task_fd; + + warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + + sem_post(&g_init_sema); + + /* Start the endless loop, waiting for then processing work requests */ + while (true) { + + /* do we need to exit ??? */ + if ((g_task_should_exit) && (g_fd >= 0)) { + /* Close the file handle to stop further queueing */ + g_fd = -1; + } + + if (!g_task_should_exit) { + /* wait for work */ + sem_wait(&g_work_queued_sema); + } + + /* Empty the work queue */ + while ((work = dequeue_work_item())) { + + switch (work->func) { + case dm_write_func: + g_func_counts[dm_write_func]++; + work->result = + _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count); + break; + + case dm_read_func: + g_func_counts[dm_read_func]++; + work->result = + _read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); + break; + + case dm_clear_func: + g_func_counts[dm_clear_func]++; + work->result = _clear(work->clear_params.item); + break; + + case dm_restart_func: + g_func_counts[dm_restart_func]++; + work->result = _restart(work->restart_params.reason); + break; + + default: /* should never happen */ + work->result = -1; + break; + } + + /* Inform the caller that work is done */ + sem_post(&work->wait_sem); + } + + /* time to go???? */ + if ((g_task_should_exit) && (g_fd < 0)) + break; + } + + close(g_task_fd); + g_task_fd = -1; + + /* Empty the work queue */ + for (;;) { + if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) + break; + + free(work); + } + + destroy_q(&g_work_q); + destroy_q(&g_free_q); + sem_destroy(&g_work_queued_sema); + + return 0; +} + +static int +start(void) +{ + int task; + + sem_init(&g_init_sema, 1, 0); + + /* start the task */ + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { + warn("task start failed"); + return -1; + } + + /* wait for the thread to actuall initialize */ + sem_wait(&g_init_sema); + sem_destroy(&g_init_sema); + + return 0; +} + +static void +status(void) +{ + /* display usage statistics */ + warnx("Writes %d", g_func_counts[dm_write_func]); + warnx("Reads %d", g_func_counts[dm_read_func]); + warnx("Clears %d", g_func_counts[dm_clear_func]); + warnx("Restarts %d", g_func_counts[dm_restart_func]); + warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size); +} + +static void +stop(void) +{ + /* Tell the worker task to shut down */ + g_task_should_exit = true; + sem_post(&g_work_queued_sema); +} + +static void +usage(void) +{ + errx(1, "usage: dataman {start|stop|status}"); +} + +int +dataman_main(int argc, char *argv[]) +{ + if (argc < 2) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (g_fd >= 0) + errx(1, "already running"); + + start(); + + if (g_fd < 0) + errx(1, "start failed"); + + exit(0); + } + + if (g_fd < 0) + errx(1, "not running"); + + if (!strcmp(argv[1], "stop")) + stop(); + else if (!strcmp(argv[1], "status")) + status(); + else + usage(); + + exit(1); +} + diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h new file mode 100644 index 000000000..a70638ccc --- /dev/null +++ b/src/modules/dataman/dataman.h @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 dataman.h + * + * DATAMANAGER driver. + */ +#ifndef _DATAMANAGER_H +#define _DATAMANAGER_H + +#include <uORB/topics/mission.h> +#include <uORB/topics/fence.h> + +#ifdef __cplusplus +extern "C" { +#endif + + /* Types of items that the data manager can store */ + typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ + DM_KEY_NUM_KEYS /* Total number of item types defined */ + } dm_item_t; + + /* The maximum number of instances for each item type */ + enum { + DM_KEY_SAFE_POINTS_MAX = 8, + DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED + }; + + /* Data persistence levels */ + typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ + } dm_persitence_t; + + /* The reason for the last reset */ + typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */ + } dm_reset_reason; + + /* Maximum size in bytes of a single item instance */ + #define DM_MAX_DATA_SIZE 126 + + /* Retrieve from the data manager store */ + __EXPORT ssize_t + dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned char index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ); + + /* write to the data manager store */ + __EXPORT ssize_t + dm_write( + dm_item_t item, /* The item type to store */ + unsigned char index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ); + + /* Retrieve from the data manager store */ + __EXPORT int + dm_clear( + dm_item_t item /* The item type to clear */ + ); + + /* Tell the data manager about the type of the last reset */ + __EXPORT int + dm_restart( + dm_reset_reason restart_type /* The last reset type */ + ); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk new file mode 100644 index 000000000..dce7a6235 --- /dev/null +++ b/src/modules/dataman/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2013 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. +# +############################################################################ + +# +# Main Navigation Controller +# + +MODULE_COMMAND = dataman + +SRCS = dataman.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d65045d68..108e9896d 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par _vCmd(this, "V_CMD"), _crMax(this, "CR_MAX"), _attPoll(), - _lastPosCmd(), + _lastMissionCmd(), _timeStamp(0) { _attPoll.fd = _att.getHandle(); @@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update() setDt(dt); // store old position command before update if new command sent - if (_posCmd.updated()) { - _lastPosCmd = _posCmd.getData(); + if (_missionCmd.updated()) { + _lastMissionCmd = _missionCmd.getData(); } // check for new updates @@ -156,9 +156,10 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { + // TODO use vehicle_control_mode here? // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + _guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || - _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between + // calculate velocity, XXX should be airspeed, + // but using ground speed for now for the purpose + // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vx * _pos.vx + @@ -184,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update() float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update() // a first binary release can be targeted. // This is not a hack, but a design choice. - /* do not limit in HIL */ + // do not limit in HIL if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + } else if (_status.main_state == MAIN_STATE_MANUAL) { _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + + } else if (_status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 3876e4630..b4dbc36b0 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -255,16 +255,16 @@ private: BlockWaypointGuidance _guide; // block params - BlockParam<float> _trimAil; - BlockParam<float> _trimElv; - BlockParam<float> _trimRdr; - BlockParam<float> _trimThr; - BlockParam<float> _trimV; - BlockParam<float> _vCmd; - BlockParam<float> _crMax; + BlockParamFloat _trimAil; + BlockParamFloat _trimElv; + BlockParamFloat _trimRdr; + BlockParamFloat _trimThr; + BlockParamFloat _trimV; + BlockParamFloat _vCmd; + BlockParamFloat _crMax; struct pollfd _attPoll; - vehicle_global_position_set_triplet_s _lastPosCmd; + mission_item_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 00a0dcd61..df9d325b3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -37,6 +37,7 @@ * Implementation of a generic attitude controller based on classic orthogonal PIDs. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> * */ @@ -62,6 +63,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_global_position.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/pid/pid.h> @@ -106,26 +108,30 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -137,6 +143,7 @@ private: float p_p; float p_d; float p_i; + float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; @@ -144,13 +151,17 @@ private: float r_p; float r_d; float r_i; + float r_ff; float r_integrator_max; float r_rmax; float y_p; float y_i; float y_d; + float y_ff; float y_roll_feedforward; float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; float airspeed_min; float airspeed_trim; @@ -163,6 +174,7 @@ private: param_t p_p; param_t p_d; param_t p_i; + param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; @@ -170,13 +182,17 @@ private: param_t r_p; param_t r_d; param_t r_i; + param_t r_ff; param_t r_integrator_max; param_t r_rmax; param_t y_p; param_t y_i; param_t y_d; + param_t y_ff; param_t y_roll_feedforward; param_t y_integrator_max; + param_t y_coordinated_min_speed; + param_t y_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -227,6 +243,11 @@ private: void vehicle_setpoint_poll(); /** + * Check for global position updates. + */ + void global_pos_poll(); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -261,11 +282,13 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), + _global_pos_sub(-1), /* publications */ _rate_sp_pub(-1), _actuators_0_pub(-1), _attitude_sp_pub(-1), + _actuators_1_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -273,31 +296,45 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false), _airspeed_valid(false) { + /* safely initialize structs */ + _att = {0}; + _accel = {0}; + _att_sp = {0}; + _manual = {0}; + _airspeed = {0}; + _vcontrol_mode = {0}; + _actuators = {0}; + _actuators_airframe = {0}; + _global_pos = {0}; + + _parameter_handles.tconst = param_find("FW_ATT_TC"); - _parameter_handles.p_p = param_find("FW_P_P"); - _parameter_handles.p_d = param_find("FW_P_D"); - _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_p = param_find("FW_PR_P"); + _parameter_handles.p_i = param_find("FW_PR_I"); + _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); + _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); - _parameter_handles.r_p = param_find("FW_R_P"); - _parameter_handles.r_d = param_find("FW_R_D"); - _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); + _parameter_handles.r_p = param_find("FW_RR_P"); + _parameter_handles.r_i = param_find("FW_RR_I"); + _parameter_handles.r_ff = param_find("FW_RR_FF"); + _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); - _parameter_handles.y_p = param_find("FW_Y_P"); - _parameter_handles.y_i = param_find("FW_Y_I"); - _parameter_handles.y_d = param_find("FW_Y_D"); - _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + _parameter_handles.y_p = param_find("FW_YR_P"); + _parameter_handles.y_i = param_find("FW_YR_I"); + _parameter_handles.y_ff = param_find("FW_YR_FF"); + _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); + _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); + /* fetch initial parameter values */ parameters_update(); } @@ -333,24 +370,26 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); - param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); - param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); - param_get(_parameter_handles.y_d, &(_parameters.y_d)); - param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); + param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); + param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -358,28 +397,29 @@ FixedwingAttitudeControl::parameters_update() /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); - _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); - _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); - _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); - _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_k_p(_parameters.p_p); + _pitch_ctrl.set_k_i(_parameters.p_i); + _pitch_ctrl.set_k_ff(_parameters.p_ff); + _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); - _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); - _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); - _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); - _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_k_p(_parameters.r_p); + _roll_ctrl.set_k_i(_parameters.r_i); + _roll_ctrl.set_k_ff(_parameters.r_ff); + _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ - _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); - _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); - _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); - _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); - _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + _yaw_ctrl.set_k_p(_parameters.y_p); + _yaw_ctrl.set_k_i(_parameters.y_i); + _yaw_ctrl.set_k_ff(_parameters.y_ff); + _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); + _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); + _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; } @@ -421,6 +461,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); +// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); return true; } @@ -453,6 +494,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } void +FixedwingAttitudeControl::global_pos_poll() +{ + /* check if there is a new global position */ + bool global_pos_updated; + orb_check(_global_pos_sub, &global_pos_updated); + + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + } +} + +void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { att_control::g_control->task_main(); @@ -476,6 +529,7 @@ FixedwingAttitudeControl::task_main() _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -551,6 +605,8 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); + global_pos_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -561,22 +617,28 @@ FixedwingAttitudeControl::task_main() lock_integrator = true; } + /* Simple handling of failsafe: deploy parachute if failsafe is on */ + if (_vcontrol_mode.flag_control_flighttermination_enabled) { + _actuators_airframe.control[1] = 1.0f; +// warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { + _actuators_airframe.control[1] = 0.0f; +// warnx("_actuators_airframe.control[1] = -1.0f;"); + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale from radians to normalized -1 .. 1 range */ - const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); - /* scale around tuning airspeed */ float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { - airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + airspeed = _parameters.airspeed_trim; } else { airspeed = _airspeed.indicated_airspeed_m_s; @@ -585,7 +647,8 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { @@ -635,46 +698,86 @@ FixedwingAttitudeControl::task_main() } } - if (isfinite(roll_sp) && isfinite(pitch_sp)) { + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if(_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; + speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; + speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + } else { + warnx("Did not get a valid R\n"); + } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u, speed_body_v, speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + if (!isfinite(roll_u)) { + warnx("roll_u %.4f", roll_u); + } - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + if (!isfinite(pitch_u)) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", + pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + } - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + if (!isfinite(yaw_u)) { + warnx("yaw_u %.4f", yaw_u); + } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + warnx("throttle_sp %.4f", throttle_sp); + } + } else { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = _yaw_ctrl.get_desired_rate(); - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.timestamp = hrt_absolute_time(); - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - } + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); } } else { @@ -692,6 +795,7 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -702,6 +806,19 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } + if (_actuators_1_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); +// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", +// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], +// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], +// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + + } else { + /* advertise and publish */ + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + } + } perf_end(_loop_perf); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 97aa275de..1c615094c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. +f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -33,41 +33,37 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file fw_att_control_params.c * - * Parameters defined by the L1 position control task + * Parameters defined by the fixed-wing attitude control task * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> #include <systemlib/param/param.h> + /* * Controller parameters, accessible via MAVLink * */ - // @DisplayName Attitude Time Constant -// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. +// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. // @Range 0.4 to 1.0 seconds, in tens of seconds PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); -// @DisplayName Proportional gain. -// @Description This defines how much the elevator input will be commanded dependend on the current pitch error. +// @DisplayName Pitch rate proportional gain. +// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error. // @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); - -// @DisplayName Damping gain. -// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. -// @Range 0.0 to 10.0, 0.1 increments -PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); -// @DisplayName Integrator gain. +// @DisplayName Pitch rate integrator gain. // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0 to 50.0 -PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); // @DisplayName Maximum positive / up pitch rate. // @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. @@ -79,58 +75,104 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); // @Range 0 to 90.0 degrees per seconds, in 1 increments PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); -// @DisplayName Pitch Integrator Anti-Windup -// @Description This limits the range in degrees the integrator can wind up to. -// @Range 0.0 to 45.0 -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +// @DisplayName Pitch rate integrator limit +// @Description The portion of the integrator part in the control surface deflection is limited to this value +// @Range 0.0 to 1 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); -// @DisplayName Roll feedforward gain. +// @DisplayName Roll to Pitch feedforward gain. // @Description This compensates during turns and ensures the nose stays level. // @Range 0.5 2.0 // @Increment 0.05 // @User User -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) -// @DisplayName Proportional Gain. -// @Description This gain controls the roll angle to roll actuator output. +// @DisplayName Roll rate proportional Gain. +// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. // @Range 10.0 200.0 // @Increment 10.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); -// @DisplayName Damping Gain -// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. -// @Range 0.0 10.0 -// @Increment 1.0 -// @User User -PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); - -// @DisplayName Integrator Gain -// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. +// @DisplayName Roll rate integrator Gain +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0.0 100.0 // @Increment 5.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); // @DisplayName Roll Integrator Anti-Windup -// @Description This limits the range in degrees the integrator can wind up to. -// @Range 0.0 to 45.0 -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +// @Description The portion of the integrator part in the control surface deflection is limited to this value. +// @Range 0.0 to 1.0 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); // @DisplayName Maximum Roll Rate // @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 0); + +// @DisplayName Yaw rate proportional gain. +// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error. +// @Range 10 to 200, 1 increments +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05); + +// @DisplayName Yaw rate integrator gain. +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. +// @Range 0 to 50.0 +PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); + +// @DisplayName Yaw rate integrator limit +// @Description The portion of the integrator part in the control surface deflection is limited to this value +// @Range 0.0 to 1 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); + +// @DisplayName Maximum Yaw Rate +// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0); + +// @DisplayName Roll rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); + +// @DisplayName Pitch rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); + +// @DisplayName Yaw rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); + +// @DisplayName Minimal speed for yaw coordination +// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); + +/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */ + +// @DisplayName Minimum Airspeed +// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); +// @DisplayName Trim Airspeed +// @Description The TECS controller tries to fly at this airspeed +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); -PARAM_DEFINE_FLOAT(FW_Y_P, 0); -PARAM_DEFINE_FLOAT(FW_Y_I, 0); -PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); -PARAM_DEFINE_FLOAT(FW_Y_D, 0); -PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); +// @DisplayName Maximum Airspeed +// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 99428ea50..26f6768cc 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * More details and acknowledgements in the referenced library headers. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> @@ -67,7 +68,7 @@ #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> @@ -83,10 +84,12 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> - +#include <mavlink/mavlink_log.h> +#include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> -#include <launchdetection/LaunchDetector.h> +#include "landingslope.h" + /** * L1 control app start / stop handling function @@ -116,12 +119,13 @@ public: int start(); private: + int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _global_set_triplet_sub; + int _mission_item_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -140,7 +144,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ struct accel_report _accel; /**< body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -161,7 +165,16 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + bool land_motor_lim; + bool land_onslope; + + /* Landingslope object */ + Landingslope landingslope; + + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -210,6 +223,17 @@ private: float throttle_land_max; float loiter_hold_radius; + + float heightrate_p; + float speedrate_p; + + float land_slope_angle; + float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + float land_heading_hold_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -244,6 +268,17 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t heightrate_p; + param_t speedrate_p; + + param_t land_slope_angle; + param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + param_t land_heading_hold_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -284,13 +319,18 @@ private: void vehicle_setpoint_poll(); /** + * Publish navigation capabilities + */ + void navigation_capabilities_publish(); + + /** * Control position. */ bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet); + const struct mission_item_triplet_s &_mission_item_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(); + void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); /** * Shim for calling task_main from task_create. @@ -322,7 +362,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _global_set_triplet_sub(-1), + _mission_item_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -342,9 +382,29 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false), + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + land_motor_lim(false), + land_onslope(false), + flare_curve_alt_last(0.0f), + _mavlink_fd(-1), launchDetector() { + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + navigation_capabilities_s _nav_capabilities = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _control_mode = {0}; + vehicle_global_position_s _global_pos = {0}; + mission_item_triplet_s _mission_item_triplet = {0}; + accel_report _accel = {0}; + + + + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -363,6 +423,13 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -375,6 +442,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -440,6 +509,16 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -452,12 +531,14 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || @@ -469,6 +550,16 @@ FixedwingPositionControl::parameters_update() return 1; } + /* Update the landing slope */ + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + + /* Update and publish the navigation capabilities */ + _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); + _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement(); + _nav_capabilities.landing_flare_length = landingslope.flare_length(); + navigation_capabilities_publish(); + + /* Update Launch Detector Parameters */ launchDetector.updateParams(); return OK; @@ -555,11 +646,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool global_sp_updated; - orb_check(_global_set_triplet_sub, &global_sp_updated); + bool mission_item_triplet_updated; + orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); + if (mission_item_triplet_updated) { + orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); _setpoint_valid = true; } } @@ -602,17 +693,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot() +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) { if (_global_pos_valid) { - /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); - /* rotate with current attitude */ + /* rotate ground speed vector with current attitude */ math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed_vector; + float ground_speed_body = yaw_vector * ground_speed; + + /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ + float distance = 0.0f; + float delta_altitude = 0.0f; + if (mission_item_triplet.previous_valid) { + distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); + delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + } else { + distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); + delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + } + + float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); + /* * Ground speed undershoot is the amount of ground velocity not reached @@ -623,20 +726,29 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; } } +void FixedwingPositionControl::navigation_capabilities_publish() +{ + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet) + const struct mission_item_triplet_s &mission_item_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(); + calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -648,7 +760,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio math::Vector3 accel_earth = _R_nb.transpose() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -665,272 +777,264 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_parameters.speed_weight); - /* execute navigation once we have a setpoint */ - if (_setpoint_valid) { - - /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - /* previous waypoint */ - math::Vector2f prev_wp; + /* current waypoint (the one currently heading for) */ + math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - if (global_triplet.previous_valid) { - prev_wp.setX(global_triplet.previous.lat / 1e7f); - prev_wp.setY(global_triplet.previous.lon / 1e7f); - - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(global_triplet.current.lat / 1e7f); - prev_wp.setY(global_triplet.current.lon / 1e7f); - - } - // XXX add RTL switch - if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + /* previous waypoint */ + math::Vector2f prev_wp; - math::Vector2f rtl_pos(_launch_lat, _launch_lon); + if (mission_item_triplet.previous_valid) { + prev_wp.setX(mission_item_triplet.previous.lat); + prev_wp.setY(mission_item_triplet.previous.lon); - _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - // XXX handle case when having arrived at home (loiter) + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(mission_item_triplet.current.lat); + prev_wp.setY(mission_item_triplet.current.lon); - } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, - global_triplet.current.loiter_direction, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, + mission_item_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - /* switch to heading hold for the last meters, continue heading hold after */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - /* heading hold, along the line connecting this and the last waypoint */ - + /* Horizontal landing control */ + /* switch to heading hold for the last meters, continue heading hold after */ + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { - // if (global_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); - // } else { + /* heading hold, along the line connecting this and the last waypoint */ - if (!land_noreturn) + if (!land_noreturn_horizontal) {//set target_bearing in first occurrence + if (mission_item_triplet.previous_valid) { + target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + } else { target_bearing = _att.yaw; - //} + } + mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + } - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -5.0f) - land_noreturn = true; + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else { + land_noreturn_horizontal = true; - /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - } + } else { - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; - } + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + } + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - // XXX this could make a great param + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.airspeed_min; - float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// } + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param - if (altitude_error > -4.0f) { + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* land with minimal speed */ + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ - _tecs.set_speed_weight(2.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, _parameters.throttle_max, throttle_land, - math::radians(-10.0f), math::radians(15.0f)); - /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + /* land with minimal speed */ - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); - /* minimize speed to approach speed */ + /* kill the throttle if param requests it */ + throttle_max = _parameters.throttle_max; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + } - } else { + } - /* normal cruise speed */ + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = mission_item_triplet.current.altitude; + land_stayonground = true; } - } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_pitch_angle_rad, + 0.0f, throttle_max, throttle_land, + flare_pitch_angle_rad, math::radians(15.0f)); - /* Perform launch detection */ - bool do_fly_takeoff = false; - warnx("Launch detection running"); - if (launchDetector.launchDetectionEnabled()) { - launchDetector.update(_accel.x); - if (launchDetector.getLaunchDetected()) { - do_fly_takeoff = true; - warnx("Launch detected. Taking off!"); - } - } else { - /* no takeoff detection --> fly */ - do_fly_takeoff = true; + if (!land_noreturn_vertical) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + land_noreturn_vertical = true; } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + flare_curve_alt_last = flare_curve_alt; - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + } else if (wp_distance < L_wp_distance) { - if (do_fly_takeoff) { - - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + /* minimize speed to approach speed, stay on landing slope */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_pitch_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + if (!land_onslope) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + land_onslope = true; + } - } else { + } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } + /* intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ + altitude_desired = landing_slope_alt_desired; + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { - throttle_max = 0.0f; + /* continue horizontally */ + altitude_desired = math::max(_global_pos.alt, L_altitude); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } - } - - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - } else if (_control_mode.flag_armed) { - - /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ - - // XXX rework with smarter state machine - - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt + 25.0f; - _loiter_hold = true; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - altitude_error = _loiter_hold_alt - _global_pos.alt; + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); + /* Perform launch detection */ + bool do_fly_takeoff = false; + warnx("Launch detection running"); + if (launchDetector.launchDetectionEnabled()) { + launchDetector.update(_accel.x); + if (launchDetector.getLaunchDetected()) { + do_fly_takeoff = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + do_fly_takeoff = true; + } - /* loiter around current position */ - _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, - 1, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 3); + if (do_fly_takeoff) { - float min_pitch; + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 15.0f) { - if (climb_out) { - min_pitch = math::radians(20.0f); + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else { - min_pitch = math::radians(_parameters.pitch_limit_min); - } + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, min_pitch, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } - if (climb_out) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + } else { + throttle_max = 0.0f; } } + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + /* reset land state */ - if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1045,7 +1149,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1107,6 +1211,11 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -1135,7 +1244,7 @@ FixedwingPositionControl::task_main() * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _global_triplet)) { + if (control_position(current_position, ground_speed, _mission_item_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1148,7 +1257,8 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } - float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + /* XXX check if radius makes sense here */ + float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { @@ -1156,11 +1266,8 @@ FixedwingPositionControl::task_main() /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; - if (_nav_capabilities_pub > 0) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); - } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); - } + navigation_capabilities_publish(); + } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 3bb872405..9f46b5170 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -111,3 +111,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); + +PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp new file mode 100644 index 000000000..b139a6397 --- /dev/null +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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: landingslope.cpp + * + */ + +#include "landingslope.h" + +#include <nuttx/config.h> +#include <stdlib.h> +#include <errno.h> +#include <math.h> +#include <unistd.h> +#include <mathlib/mathlib.h> + +void Landingslope::update(float landing_slope_angle_rad, + float flare_relative_alt, + float motor_lim_horizontal_distance, + float H1_virt) +{ + + _landing_slope_angle_rad = landing_slope_angle_rad; + _flare_relative_alt = flare_relative_alt; + _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _H1_virt = H1_virt; + + calculateSlopeValues(); +} + +void Landingslope::calculateSlopeValues() +{ + _H0 = _flare_relative_alt + _H1_virt; + _d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad); + _flare_constant = (_H0 * _d1)/_flare_relative_alt; + _flare_length = - logf(_H1_virt/_H0) * _flare_constant; + _horizontal_slope_displacement = (_flare_length - _d1); +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +{ + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +{ + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + +} + diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h new file mode 100644 index 000000000..1a149fc7c --- /dev/null +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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: landingslope.h + * + */ + +#ifndef LANDINGSLOPE_H_ +#define LANDINGSLOPE_H_ + +#include <math.h> +#include <systemlib/err.h> + +class Landingslope +{ +private: + /* see Documentation/fw_landing.png for a plot of the landing slope */ + float _landing_slope_angle_rad; /**< phi in the plot */ + float _flare_relative_alt; /**< h_flare,rel in the plot */ + float _motor_lim_horizontal_distance; + float _H1_virt; /**< H1 in the plot */ + float _H0; /**< h_flare,rel + H1 in the plot */ + float _d1; /**< d1 in the plot */ + float _flare_constant; + float _flare_length; /**< d1 + delta d in the plot */ + float _horizontal_slope_displacement; /**< delta d in the plot */ + + void calculateSlopeValues(); + +public: + Landingslope() {} + ~Landingslope() {} + + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + } + + /** + * + * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude + */ + __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement; + + } + + + float getFlareCurveAltitude(float wp_distance, float wp_altitude); + + void update(float landing_slope_angle_rad, + float flare_relative_alt, + float motor_lim_horizontal_distance, + float H1_virt); + + + inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} + inline float flare_relative_alt() {return _flare_relative_alt;} + inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float H1_virt() {return _H1_virt;} + inline float H0() {return _H0;} + inline float d1() {return _d1;} + inline float flare_constant() {return _flare_constant;} + inline float flare_length() {return _flare_length;} + inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;} + +}; + + +#endif /* LANDINGSLOPE_H_ */ diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index b00b9aa5a..cf419ec7e 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ - fw_pos_control_l1_params.c + fw_pos_control_l1_params.c \ + landingslope.cpp diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c10e297b..4c38cf35a 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,12 +68,13 @@ #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "util.h" #include "waypoints.h" #include "mavlink_parameters.h" +#include <uORB/topics/mission_result.h> + /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); @@ -219,6 +220,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO use control_mode topic + /* if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { @@ -232,6 +235,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } + */ } *mavlink_custom_mode = custom_mode.data; @@ -644,6 +648,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + thread_running = true; /* arm counter to go off immediately */ @@ -690,25 +698,36 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); + } + } + + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c51a6de08..771989430 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -72,13 +72,13 @@ #include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> #include <commander/px4_custom_mode.h> +#include <geo/geo.h> __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "mavlink_parameters.h" #include "util.h" @@ -99,11 +99,13 @@ static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; struct vehicle_global_position_s hil_global_pos; +struct vehicle_local_position_s hil_local_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_local_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; @@ -126,6 +128,11 @@ static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; +// variables for HIL reference position +static int32_t lat0 = 0; +static int32_t lon0 = 0; +static double alt0 = 0; + static void handle_message(mavlink_message_t *msg) { @@ -374,16 +381,15 @@ handle_message(mavlink_message_t *msg) /* hil gyro */ static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = hil_counter; /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; static const float mg2ms2 = 9.8f / 1000.0f; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -393,6 +399,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = hil_counter; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -401,7 +408,6 @@ handle_message(mavlink_message_t *msg) /* magnetometer */ float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; @@ -411,15 +417,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = hil_counter; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - - hil_sensors.gyro_counter = hil_counter; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.baro_counter = hil_counter; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa @@ -621,24 +625,61 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - hil_global_pos.valid = true; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; - - /* set timestamp and notify processes (broadcast) */ - hil_global_pos.timestamp = hrt_absolute_time(); + uint64_t timestamp = hrt_absolute_time(); + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + // global position packet + hil_global_pos.timestamp = timestamp; + hil_global_pos.valid = true; + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } + // publish local position + if (pub_hil_local_pos > 0) { + float x; + float y; + bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + double lat = hil_state.lat*1e-7; + double lon = hil_state.lon*1e-7; + map_projection_project(lat, lon, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = alt0 - hil_state.alt/1000.0f; + hil_local_pos.vx = hil_state.vx/100.0f; + hil_local_pos.vy = hil_state.vy/100.0f; + hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = alt0; + hil_local_pos.landed = landed; + orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { + pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + lat0 = hil_state.lat; + lon0 = hil_state.lon; + alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + } + /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); math::Dcm C_nb(q); @@ -802,7 +843,7 @@ receive_thread(void *arg) handle_message(&msg); /* handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c deleted file mode 100644 index fa23f996f..000000000 --- a/src/modules/mavlink/missionlib.c +++ /dev/null @@ -1,390 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * 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 missionlib.h - * MAVLink missionlib components - */ - -// XXX trim includes -#include <nuttx/config.h> -#include <unistd.h> -#include <pthread.h> -#include <stdio.h> -#include <math.h> -#include <stdbool.h> -#include <fcntl.h> -#include <mqueue.h> -#include <string.h> -#include "mavlink_bridge_header.h" -#include <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <stdlib.h> -#include <poll.h> - -#include <systemlib/err.h> -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <mavlink/mavlink_log.h> - -#include "geo/geo.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; -static uint64_t loiter_start_time; - -static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, - struct vehicle_global_position_setpoint_s *sp); - -int -mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); - return 0; -} - -int -mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -/** - * Set special vehicle setpoint fields based on current mission item. - * - * @return true if the mission item could be interpreted - * successfully, it return false on failure. - */ -bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, - struct vehicle_global_position_setpoint_s *sp) -{ - switch (command) { - case MAV_CMD_NAV_LOITER_UNLIM: - sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - break; - case MAV_CMD_NAV_LOITER_TIME: - sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - loiter_start_time = hrt_absolute_time(); - break; - // case MAV_CMD_NAV_LOITER_TURNS: - // sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT; - // break; - case MAV_CMD_NAV_WAYPOINT: - sp->nav_cmd = NAV_CMD_WAYPOINT; - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - break; - case MAV_CMD_NAV_LAND: - sp->nav_cmd = NAV_CMD_LAND; - break; - case MAV_CMD_NAV_TAKEOFF: - sp->nav_cmd = NAV_CMD_TAKEOFF; - break; - default: - /* abort */ - return false; - } - - sp->loiter_radius = param3; - sp->loiter_direction = (param3 >= 0) ? 1 : -1; - - sp->param1 = param1; - sp->param2 = param2; - sp->param3 = param3; - sp->param4 = param4; - - - /* define the turn distance */ - float orbit = 15.0f; - - if (command == (int)MAV_CMD_NAV_WAYPOINT) { - - orbit = param2; - - } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || - command == (int)MAV_CMD_NAV_LOITER_TIME || - command == (int)MAV_CMD_NAV_LOITER_UNLIM) { - - orbit = param3; - } else { - - // XXX set default orbit via param - // 15 initialized above - } - - sp->turn_distance_xy = orbit; - sp->turn_distance_z = orbit; -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) -{ - static orb_advert_t global_position_setpoint_pub = -1; - static orb_advert_t global_position_set_triplet_pub = -1; - static orb_advert_t local_position_setpoint_pub = -1; - static unsigned last_waypoint_index = -1; - char buf[50] = {0}; - - // XXX include check if WP is supported, jump to next if not - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = false; - sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); - set_special_fields(param1, param2, param3, param4, command, &sp); - - /* Initialize setpoint publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - - /* fill triplet: previous, current, next waypoint */ - struct vehicle_global_position_set_triplet_s triplet; - - /* current waypoint is same as sp */ - memcpy(&(triplet.current), &sp, sizeof(sp)); - - /* - * Check if previous WP (in mission, not in execution order) - * is available and identify correct index - */ - int last_setpoint_index = -1; - bool last_setpoint_valid = false; - - if (index > 0) { - last_setpoint_index = index - 1; - } - - while (last_setpoint_index >= 0) { - - if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && - (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - last_setpoint_valid = true; - break; - } - - last_setpoint_index--; - } - - /* - * Check if next WP (in mission, not in execution order) - * is available and identify correct index - */ - int next_setpoint_index = -1; - bool next_setpoint_valid = false; - - /* next waypoint */ - if (wpm->size > 1) { - next_setpoint_index = index + 1; - } - - while (next_setpoint_index < wpm->size - 1) { - - if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - next_setpoint_valid = true; - break; - } - - next_setpoint_index++; - } - - /* populate last and next */ - - triplet.previous_valid = false; - triplet.next_valid = false; - - if (last_setpoint_valid) { - triplet.previous_valid = true; - struct vehicle_global_position_setpoint_s sp; - sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; - sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; - sp.altitude = wpm->waypoints[last_setpoint_index].z; - sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; - set_special_fields(wpm->waypoints[last_setpoint_index].param1, - wpm->waypoints[last_setpoint_index].param2, - wpm->waypoints[last_setpoint_index].param3, - wpm->waypoints[last_setpoint_index].param4, - wpm->waypoints[last_setpoint_index].command, &sp); - memcpy(&(triplet.previous), &sp, sizeof(sp)); - } - - if (next_setpoint_valid) { - triplet.next_valid = true; - struct vehicle_global_position_setpoint_s sp; - sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; - sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; - sp.altitude = wpm->waypoints[next_setpoint_index].z; - sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; - set_special_fields(wpm->waypoints[next_setpoint_index].param1, - wpm->waypoints[next_setpoint_index].param2, - wpm->waypoints[next_setpoint_index].param3, - wpm->waypoints[next_setpoint_index].param4, - wpm->waypoints[next_setpoint_index].command, &sp); - memcpy(&(triplet.next), &sp, sizeof(sp)); - } - - /* Initialize triplet publication if necessary */ - if (global_position_set_triplet_pub < 0) { - global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); - - } else { - orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); - } - - sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - set_special_fields(param1, param2, param3, param4, command, &sp); - - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - - - sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - sp.y = param6_lon_y; - sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - - /* Initialize publication if necessary */ - if (local_position_setpoint_pub < 0) { - local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); - } - - sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - } else { - warnx("non-navigation WP, ignoring"); - mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); - return; - } - - /* only set this for known waypoint types (non-navigation types would have returned earlier) */ - last_waypoint_index = index; - - mavlink_missionlib_send_gcs_string(buf); - printf("%s\n", buf); - //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); -} diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 5d3d6a73c..89a097c24 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ - missionlib.c \ mavlink_parameters.c \ mavlink_receiver.cpp \ orb_listener.c \ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34f..28478a803 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,12 +54,12 @@ #include <sys/prctl.h> #include <stdlib.h> #include <poll.h> +#include <lib/geo/geo.h> #include <mavlink/mavlink_log.h> #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "util.h" @@ -72,7 +72,6 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_effective_0; struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct airspeed_s airspeed; @@ -119,7 +118,6 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); -static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -137,7 +135,7 @@ static const struct listener listeners[] = { {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, - {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.triplet_sub, 0}, {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, @@ -147,7 +145,6 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, - {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -242,16 +239,29 @@ l_vehicle_attitude(const struct listener *l) att.rollspeed, att.pitchspeed, att.yawspeed); - + /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } + + /* send quaternion values if it exists */ + if(att.q_valid) { + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); + } } attitude_counter++; @@ -266,13 +276,7 @@ l_vehicle_gps_position(const struct listener *l) orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = gps.cog_rad; - - if (cog_deg > M_PI_F) - cog_deg -= 2.0f * M_PI_F; - - cog_deg *= M_RAD_TO_DEG_F; - + float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, @@ -365,28 +369,16 @@ l_global_position(const struct listener *l) /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - uint64_t timestamp = global_pos.timestamp; - int32_t lat = global_pos.lat; - int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt * 1000); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); - int16_t vx = (int16_t)(global_pos.vx * 100.0f); - int16_t vy = (int16_t)(global_pos.vy * 100.0f); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, - timestamp / 1000, - lat, - lon, - alt, - relative_alt, - vx, - vy, - vz, - hdg); + global_pos.timestamp / 1000, + global_pos.lat, + global_pos.lon, + global_pos.alt * 1000.0f, + global_pos.relative_alt * 1000.0f, + global_pos.vx * 100.0f, + global_pos.vy * 100.0f, + global_pos.vz * 100.0f, + _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void @@ -409,23 +401,24 @@ l_local_position(const struct listener *l) void l_global_position_setpoint(const struct listener *l) { - struct vehicle_global_position_setpoint_s global_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + struct mission_item_triplet_s triplet; + orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + + if (!triplet.current_valid) + return; - if (global_sp.altitude_is_relative) + if (triplet.current.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, - global_sp.lat, - global_sp.lon, - global_sp.altitude, - global_sp.yaw); + (int32_t)(triplet.current.lat * 1e7d), + (int32_t)(triplet.current.lon * 1e7d), + (int32_t)(triplet.current.altitude * 1e3f), + (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } void @@ -506,8 +499,8 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[6], act_outputs.output[7]); - /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + /* only send in HIL mode and only send first group for HIL */ + if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -604,32 +597,6 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls_effective(const struct listener *l) -{ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); - - if (gcs_link) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl0 ", - actuators_effective_0.control_effective[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl1 ", - actuators_effective_0.control_effective[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl2 ", - actuators_effective_0.control_effective[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl3 ", - actuators_effective_0.control_effective[3]); - } -} - -void l_vehicle_attitude_controls(const struct listener *l) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); @@ -689,7 +656,7 @@ l_home(const struct listener *l) orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void @@ -803,9 +770,9 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ - + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ + /* --- LOCAL SETPOINT VALUE --- */ mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ @@ -839,9 +806,6 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 91773843b..9000728cb 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -45,14 +45,15 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> @@ -86,7 +87,7 @@ struct mavlink_subscriptions { int local_pos_sub; int spa_sub; int spl_sub; - int spg_sub; + int triplet_sub; int debug_key_value; int input_rc_sub; int optical_flow; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7e4a2688f..2ff11e813 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> @@ -44,29 +44,155 @@ #include <sys/prctl.h> #include <unistd.h> #include <stdio.h> - #include "mavlink_bridge_header.h" -#include "missionlib.h" #include "waypoints.h" #include "util.h" +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <geo/geo.h> +#include <dataman/dataman.h> +#include <drivers/drv_hrt.h> +#include <systemlib/err.h> -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif +bool verbose = true; -bool debug = false; -bool verbose = false; +orb_advert_t mission_pub = -1; +struct mission_s mission; +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; -#define MAVLINK_WPM_NO_PRINTF +void +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); +} -void mavlink_wpm_init(mavlink_wpm_storage *state) + + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_missionlib_send_message(&msg); + return OK; + + } else { + return 1; + } +} + +void publish_mission() +{ + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } +} + +int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param2; + break; + default: + mission_item->radius = mavlink_mission_item->param2; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = mavlink_mission_item->command; + + mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + return OK; +} + +int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { - // Set all waypoints to zero + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param2 = mission_item->pitch_min; + break; + default: + mavlink_mission_item->param2 = mission_item->radius; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} - // Set count to zero +void mavlink_wpm_init(mavlink_wpm_storage *state) +{ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; state->current_state = MAVLINK_WPM_STATE_IDLE; @@ -75,14 +201,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - + state->current_dataman_id = 0; } /* @@ -93,25 +212,14 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) mavlink_message_t msg; mavlink_mission_ack_t wpa; - wpa.target_system = wpm->current_partner_sysid; - wpa.target_component = wpm->current_partner_compid; + wpa.target_system = sysid; + wpa.target_component = compid; wpa.type = type; mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - -#endif - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); - } + if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } /* @@ -126,45 +234,19 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) void mavlink_wpm_send_waypoint_current(uint16_t seq) { if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_message_t msg; mavlink_mission_current_t wpc; - wpc.seq = cur->seq; + wpc.seq = seq; mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, - cur->param2, cur->param3, cur->param4, cur->x, - cur->y, cur->z, cur->frame, cur->command); - - wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (verbose) warnx("ERROR: index out of bounds"); } } @@ -173,34 +255,47 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou mavlink_message_t msg; mavlink_mission_count_t wpc; - wpc.target_system = wpm->current_partner_sysid; - wpc.target_component = wpm->current_partner_compid; - wpc.count = count; + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = mission.count; mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - if (seq < wpm->size) { - mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - wp->target_system = wpm->current_partner_sysid; - wp->target_component = wpm->current_partner_compid; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); - mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_current; + + if (wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, seq, &mission_item, len) == len) { - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: could not read WP%u", seq); } } @@ -209,18 +304,17 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s if (seq < wpm->max_size) { mavlink_message_t msg; mavlink_mission_request_t wpr; - wpr.target_system = wpm->current_partner_sysid; - wpr.target_component = wpm->current_partner_compid; + wpr.target_system = sysid; + wpr.target_component = compid; wpr.seq = seq; mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); + if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); } } @@ -241,293 +335,33 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -/* - * Calculate distance in global frame. - * - * The distance calculation is based on the WGS84 geoid (GPS) - */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) -{ - - if (seq < wpm->size) { - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - - double current_x_rad = wp->x / 180.0 * M_PI; - double current_y_rad = wp->y / 180.0 * M_PI; - double x_rad = lat / 180.0 * M_PI; - double y_rad = lon / 180.0 * M_PI; - - double d_lat = x_rad - current_x_rad; - double d_lon = y_rad - current_y_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - float dxy = radius_earth * c; - float dz = alt - wp->z; - - *dist_xy = fabsf(dxy); - *dist_z = fabsf(dz); - - return sqrtf(dxy * dxy + dz * dz); - - } else { - return -1.0f; - } - -} - -/* - * Calculate distance in local frame (NED) - */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - float dx = (cur->x - x); - float dy = (cur->y - y); - float dz = (cur->z - z); - - *dist_xy = sqrtf(dx * dx + dy * dy); - *dist_z = fabsf(dz); - - return sqrtf(dx * dx + dy * dy + dz * dz); - - } else { - return -1.0f; - } -} - -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) -{ - static uint16_t counter; - - if ((!global_pos->valid && !local_pos->xy_valid) || - /* no waypoint */ - wpm->size == 0) { - /* nothing to check here, return */ - return; - } - - if (wpm->current_active_wp_id < wpm->size) { - - float orbit; - if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { - - orbit = wpm->waypoints[wpm->current_active_wp_id].param2; - - } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { - - orbit = wpm->waypoints[wpm->current_active_wp_id].param3; - } else { - - // XXX set default orbit via param - orbit = 15.0f; - } - - /* keep vertical orbit */ - float vertical_switch_distance = orbit; - - /* Take the larger turn distance - orbit or turn_distance */ - if (orbit < turn_distance) - orbit = turn_distance; - - int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - float dist = -1.0f; - - float dist_xy = -1.0f; - float dist_z = -1.0f; - - if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); - - } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); - - } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); - - } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - /* Check if conditions of mission item are satisfied */ - // XXX TODO - } - - if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { - wpm->pos_reached = true; - } - - // check if required yaw reached - float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); - float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); - if (fabsf(yaw_err) < 0.05f) { - wpm->yaw_reached = true; - } - } - - //check if the current waypoint was reached - if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { - if (wpm->current_active_wp_id < wpm->size) { - mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); - - if (wpm->timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm->timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - - bool time_elapsed = false; - - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { - time_elapsed = true; - } - - if (time_elapsed) { - - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } - - if (cur_wp->autocontinue) { - - cur_wp->current = 0; - - float navigation_lat = -1.0f; - float navigation_lon = -1.0f; - float navigation_alt = -1.0f; - int navigation_frame = -1; - - /* initialize to current position in case we don't find a suitable navigation waypoint */ - if (global_pos->valid) { - navigation_lat = global_pos->lat/1e7; - navigation_lon = global_pos->lon/1e7; - navigation_alt = global_pos->alt; - navigation_frame = MAV_FRAME_GLOBAL; - } else if (local_pos->xy_valid && local_pos->z_valid) { - navigation_lat = local_pos->x; - navigation_lon = local_pos->y; - navigation_alt = local_pos->z; - navigation_frame = MAV_FRAME_LOCAL_NED; - } - - /* guard against missions without final land waypoint */ - /* only accept supported navigation waypoints, skip unknown ones */ - do { - - /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ - if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { - - /* this is a navigation waypoint */ - navigation_frame = cur_wp->frame; - navigation_lat = cur_wp->x; - navigation_lon = cur_wp->y; - navigation_alt = cur_wp->z; - } - - if (wpm->current_active_wp_id == wpm->size - 1) { - - /* if we're not landing at the last nav waypoint, we're falling back to loiter */ - if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { - /* the last waypoint was reached, if auto continue is - * activated AND it is NOT a land waypoint, keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - } - - /* we risk an endless loop for missions without navigation waypoints, abort. */ - break; - - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } - - } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); - - // Fly to next waypoint - wpm->timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->waypoints[wpm->current_active_wp_id].current = true; - wpm->pos_reached = false; - wpm->yaw_reached = false; - printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); - } - } - } - - } else { - wpm->timestamp_lastoutside_orbit = now; - } - - counter++; + if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) +void mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); -#else + mavlink_missionlib_send_gcs_string("Operation timeout"); - if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); + if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_count = 0; wpm->current_partner_sysid = 0; wpm->current_partner_compid = 0; - wpm->current_wp_id = -1; - - if (wpm->size == 0) { - wpm->current_active_wp_id = -1; - } } - - check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); - - return OK; } -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) +void mavlink_wpm_message_handler(const mavlink_message_t *msg) { - uint64_t now = mavlink_missionlib_get_system_timestamp(); + uint64_t now = hrt_absolute_time(); switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { + case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); @@ -537,8 +371,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } @@ -546,12 +378,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); @@ -560,44 +393,32 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < wpm->size) { - // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - wpm->current_active_wp_id = wpc.seq; - uint32_t i; - - for (i = 0; i < wpm->size; i++) { - if (i == wpm->current_active_wp_id) { - wpm->waypoints[i].current = true; - } else { - wpm->waypoints[i].current = false; - } - } - - mavlink_missionlib_send_gcs_string("NEW WP SET"); - - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; + mission.current_index = wpc.seq; + publish_mission(); + + mavlink_wpm_send_waypoint_current(wpc.seq); } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (verbose) warnx("IGN WP CURR CMD: Not in list"); } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); + if (verbose) warnx("IGN WP CURR CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("REJ. WP CMD: target id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); @@ -606,437 +427,304 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->size > 0) { - //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; } else { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + if (verbose) warnx("No waypoints send"); } wpm->current_count = wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); } else { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (verbose) warnx("IGN REQUEST LIST: Busy"); } } else { - // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); + if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST: { + case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); -#else + if (wpr.seq >= wpm->size) { - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + break; + } -#endif - } + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else + if (wpr.seq == 0) { + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + if (verbose) warnx("REJ. WP CMD: First id != 0"); + break; + } - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { -#endif - } + if (wpr.seq == wpm->current_wp_id) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + } else if (wpr.seq == wpm->current_wp_id + 1) { -#endif + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + break; } - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm->current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); - } else { - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - break; - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - -#endif - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - -#endif - - } else if (wpr.seq >= wpm->size) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + break; + } -#endif - } + wpm->current_wp_id = wpr.seq; + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else + if (wpr.seq < wpm->size) { - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); -#endif - } - } + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } + } else { //we we're target but already communicating with someone else if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); -#endif + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } - } - break; } - case MAVLINK_MSG_ID_MISSION_COUNT: { + case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { -// printf("wpc count in: %d\n",wpc.count); -// printf("Comp id: %d\n",msg->compid); -// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); - - if (wpc.count > 0) { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); -#else + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); + break; + } -#endif - } + if (wpc.count == 0) { + mavlink_missionlib_send_gcs_string("COUNT 0"); + if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + break; + } + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); -#else + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); -#endif - } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; + if (wpm->current_wp_id == 0) { + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + } + } else { + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + } + } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); -#else + mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + } + } + break; - if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } + if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + wpm->timestamp_lastaction = now; - } else if (wpc.count == 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("COUNT 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - break; + /* + * ensure that we are in the correct state and that the first waypoint has id 0 + * and the following waypoints have the correct ids + */ - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CMD"); -#else + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { -#endif + if (wp.seq >= wpm->current_count) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); + break; } - } else { - if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); + if (wp.seq != wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + break; + } + } -#endif + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else + struct mission_item_s mission_item; - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); -#endif - } + if (ret != OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; } - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else + ssize_t len = sizeof(struct mission_item_s); - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + dm_item_t dm_next; -#endif - } + if (wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } - } - break; + if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); + if (wp.current) { + mission.current_index = wp.seq; + } - mavlink_missionlib_send_gcs_string("GOT WP"); -// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); -// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); + wpm->current_wp_id = wp.seq + 1; -// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); - wpm->timestamp_lastaction = now; + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); -// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); - -// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { - //mavlink_missionlib_send_gcs_string("DEBUG 2"); - -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); -// - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); -// printf("WP seq: %d\n",wp.seq); - wpm->current_wp_id = wp.seq + 1; - - // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); -// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - -// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - mavlink_missionlib_send_gcs_string("GOT ALL WPS"); - // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - - if (wpm->current_active_wp_id > wpm->rcv_size - 1) { - wpm->current_active_wp_id = wpm->rcv_size - 1; - } - - // switch the waypoints list - // FIXME CHECK!!! - uint32_t i; - - for (i = 0; i < wpm->current_count; ++i) { - wpm->waypoints[i] = wpm->rcv_waypoints[i]; - } - - wpm->size = wpm->current_count; - - //get the new current waypoint - - for (i = 0; i < wpm->size; i++) { - if (wpm->waypoints[i].current == 1) { - wpm->current_active_wp_id = i; - //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm->size) { - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - wpm->timestamp_firstinside_orbit = 0; - } + mission.count = wpm->current_count; + + publish_mission(); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_dataman_id = mission.dataman_id; + wpm->size = wpm->current_count; - } else { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - } + wpm->current_state = MAVLINK_WPM_STATE_IDLE; } else { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (!(wp.seq == 0)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm->current_wp_id)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (!(wp.seq < wpm->current_count)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } + } else { - //we we're target but already communicating with someone else - if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); - } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; + + wpm->size = 0; + + /* prepare mission topic */ + mission.dataman_id = -1; + mission.count = 0; + mission.current_index = -1; + publish_mission(); + + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + } + + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); + if (verbose) warnx("IGN WP CLEAR CMD: Busy"); + } - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm->size = 0; - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + + mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; } default: { - // if (debug) // printf("Waypoint: received message of unknown type"); + /* other messages might should get caught by mavlink and others */ break; } } - - // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index d7d6b31dc..f8b58c7d9 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -46,18 +46,10 @@ or in the same folder as this source file */ #include <v1.0/mavlink_types.h> - -// #ifndef MAVLINK_SEND_UART_BYTES -// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) -// #endif -//extern mavlink_system_t mavlink_system; #include "mavlink_bridge_header.h" #include <stdbool.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/mission.h> -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -78,55 +70,43 @@ enum MAVLINK_WPM_CODES { }; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif +#define MAVLINK_WPM_MAX_WP_COUNT 255 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 struct mavlink_wpm_storage { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif uint16_t size; uint16_t max_size; - uint16_t rcv_size; enum MAVLINK_WPM_STATES current_state; int16_t current_wp_id; ///< Waypoint in current transmission - int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards uint16_t current_count; uint8_t current_partner_sysid; uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; + int current_dataman_id; }; typedef struct mavlink_wpm_storage mavlink_wpm_storage; +int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); +int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void mavlink_wpm_init(mavlink_wpm_storage *state); -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , - struct vehicle_local_position_s *local_pos); +void mavlink_waypoint_eventloop(uint64_t now); +void mavlink_wpm_message_handler(const mavlink_message_t *msg); extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + +void mavlink_missionlib_send_message(mavlink_message_t *msg); +int mavlink_missionlib_send_gcs_string(const char *string); + #endif /* WAYPOINTS_H_ */ diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 0edb53a3e..ab9ce45f3 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + // TODO fix navigation state, use control_mode topic + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 1b49c9ce4..86bfa26f2 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 60a211817..111e9197f 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[]) } } else { - if (!control_mode.flag_control_auto_enabled) { + if (!control_mode.flag_control_attitude_enabled) { /* no control, try to stay on place */ if (!control_mode.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c09..2ca650420 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,8 +61,8 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <systemlib/systemlib.h> #include <systemlib/pid/pid.h> #include <mavlink/mavlink_log.h> @@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&global_pos_sp, 0, sizeof(global_pos_sp)); + struct mission_item_triplet_s triplet; + memset(&triplet, 0, sizeof(triplet)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(global_pos_sp_sub, &updated); + orb_check(mission_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - global_pos_sp_valid = true; + orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); + global_pos_sp_valid = triplet.current_valid; reset_mission_sp = true; } @@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; @@ -418,142 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { - att_sp.yaw_body = global_pos_sp.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } } + /* AUTO not implemented */ /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp new file mode 100644 index 000000000..25b2636bb --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 mission_feasibility_checker.cpp + * Provides checks if mission is feasible given the navigation capabilities + */ + +#include "mission_feasibility_checker.h" + +#include <geo/geo.h> +#include <math.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> +#include <fw_pos_control_l1/landingslope.h> +#include <systemlib/err.h> +#include <stdio.h> +#include <fcntl.h> +#include <errno.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +{ + _nav_caps = {0}; +} + + +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +{ + /* Init if not done yet */ + init(); + + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + + if (isRotarywing) + return checkMissionFeasibleRotarywing(dm_current, nItems); + else + return checkMissionFeasibleFixedwing(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +{ + + return checkGeofence(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +{ + /* Update fixed wing navigation capabilites */ + updateNavigationCapabilities(); +// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); + + return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); +} + +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +{ + //xxx: check geofence + return true; +} + +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +{ + /* Go through all mission items and search for a landing waypoint + * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + + + for (size_t i = 0; i < nItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + struct mission_item_s missionitem_previous; + if (i != 0) { + if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float delta_altitude = missionitem.altitude - missionitem_previous.altitude; +// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", +// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); +// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", +// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); + + if (wp_distance > _nav_caps.landing_flare_length) { + /* Last wp is before flare region */ + + if (delta_altitude < 0) { + if (missionitem_previous.altitude <= slope_alt_req) { + /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ + return true; + } else { + /* Landing waypoint is above altitude of slope at the given waypoint distance */ + mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); + mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + (double)(slope_alt_req), + (double)(wp_distance_req - wp_distance)); + return false; + } + } else { + /* Landing waypoint is above last waypoint */ + mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + return false; + } + } else { + /* Last wp is in flare region */ + //xxx give recommendations + mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + return false; + } + } else { + mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + return false; + } + } + } + + +// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + +void MissionFeasibilityChecker::updateNavigationCapabilities() +{ + int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + +void MissionFeasibilityChecker::init() +{ + if (!_initDone) { + + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + + _initDone = true; + } +} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h new file mode 100644 index 000000000..7d1cc2f8a --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 mission_feasibility_checker.h + * Provides checks if mission is feasible given the navigation capabilities + */ +#ifndef MISSION_FEASIBILITY_CHECKER_H_ +#define MISSION_FEASIBILITY_CHECKER_H_ + +#include <unistd.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/navigation_capabilities.h> +#include <dataman/dataman.h> + + +class MissionFeasibilityChecker +{ +private: + int _mavlink_fd; + + int _capabilities_sub; + struct navigation_capabilities_s _nav_caps; + + bool _initDone; + void init(); + + /* Checks for all airframes */ + bool checkGeofence(dm_item_t dm_current, size_t nItems); + + /* Checks specific to fixedwing airframes */ + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + void updateNavigationCapabilities(); + + /* Checks specific to rotarywing airframes */ + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); +public: + + MissionFeasibilityChecker(); + ~MissionFeasibilityChecker() {} + + /* + * Returns true if mission is feasible and false otherwise + */ + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + +}; + + +#endif /* MISSION_FEASIBILITY_CHECKER_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 0404b06c7..6be4e87a0 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,4 +38,8 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c + navigator_params.c \ + navigator_mission.cpp \ + mission_feasibility_checker.cpp + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444a..0a0ee2541 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,7 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +38,7 @@ * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. + * Published the mission item triplet for the position controller. */ #include <nuttx/config.h> @@ -48,26 +51,40 @@ #include <math.h> #include <poll.h> #include <time.h> +#include <sys/ioctl.h> +#include <drivers/device/device.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> -#include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/mission_item_triplet.h> +#include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> +#include <uORB/topics/fence.h> +#include <uORB/topics/navigation_capabilities.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <geo/geo.h> +#include <systemlib/state_table.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> +#include <geo/geo.h> #include <mathlib/mathlib.h> +#include <dataman/dataman.h> +#include <mavlink/mavlink_log.h> + +#include "navigator_mission.h" +#include "mission_feasibility_checker.h" + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; /** * navigator app start / stop handling function @@ -76,7 +93,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator +class Navigator : public StateTable { public: /** @@ -85,103 +102,141 @@ public: Navigator(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the navigators task. */ ~Navigator(); /** - * Start the sensors task. + * Start the navigator task. * * @return OK on success. */ int start(); + /** + * Display the navigator status. + */ + void status(); + + /** + * Load fence parameters. + */ + bool load_fence(unsigned vertices); + + /** + * Specify fence vertex position. + */ + void fence_point(int argc, char *argv[]); + private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ + int _navigator_task; /**< task handle for sensor task */ + + int _mavlink_fd; - int _global_pos_sub; - int _att_sub; /**< vehicle attitude subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; + int _params_sub; /**< notification of parameter updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< position setpoint */ + orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _fence_pub; /**< publish fence topic */ + orb_advert_t _mission_result_pub; /**< publish mission result topic */ + orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct home_position_s _home_pos; /**< home position for RTL */ + struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ perf_counter_t _loop_perf; /**< loop performance counter */ + + struct fence_s _fence; /**< storage for fence vertices */ + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ + struct navigation_capabilities_s _nav_caps; - /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; + class Mission _mission; - struct { - float throttle_cruise; - } _parameters; /**< local copies of interesting parameters */ + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + uint64_t _time_first_inside_orbit; + + MissionFeasibilityChecker missionFeasiblityChecker; + + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ struct { - param_t throttle_cruise; + float min_altitude; + float loiter_radius; + int onboard_mission_enabled; + } _parameters; /**< local copies of parameters */ - } _parameter_handles; /**< handles for interesting parameters */ + struct { + param_t min_altitude; + param_t loiter_radius; + param_t onboard_mission_enabled; + + } _parameter_handles; /**< handles for parameters */ + + enum Event { + EVENT_NONE_REQUESTED, + EVENT_LOITER_REQUESTED, + EVENT_MISSION_REQUESTED, + EVENT_RTL_REQUESTED, + EVENT_MISSION_FINISHED, + EVENT_MISSION_CHANGED, + EVENT_HOME_POSITION_CHANGED, + MAX_EVENT + }; + /** + * State machine transition table + */ + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; /** * Update our local parameter cache. */ - int parameters_update(); + void parameters_update(); /** - * Update control outputs - * + * Retrieve global position */ - void control_update(); + void global_position_update(); /** - * Check for changes in vehicle status. + * Retrieve home position */ - void vehicle_status_poll(); + void home_position_update(); /** - * Check for position updates. + * Retreive navigation capabilities */ - void vehicle_attitude_poll(); + void navigation_capabilities_update(); /** - * Check for set triplet updates. + * Retrieve offboard mission. */ - void mission_poll(); + void offboard_mission_update(bool isrotaryWing); /** - * Control throttle. + * Retrieve onboard mission. */ - float control_throttle(float energy_error); + void onboard_mission_update(); /** - * Control pitch. + * Retrieve vehicle status */ - float control_pitch(float altitude_error); + void vehicle_status_update(); - void calculate_airspeed_errors(); - void calculate_gndspeed_undershoot(); - void calculate_altitude_error(); /** * Shim for calling task_main from task_create. @@ -192,6 +247,68 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + void publish_fence(unsigned vertices); + + void publish_safepoints(unsigned points); + + bool fence_valid(const struct fence_s &fence); + + /** + * Functions that are triggered when a new state is entered. + */ + void start_none(); + void start_loiter(); + void start_mission(); + void start_mission_loiter(); + void start_rtl(); + void start_rtl_loiter(); + + /** + * Guards offboard mission + */ + bool offboard_mission_available(unsigned relative_index); + + /** + * Guards onboard mission + */ + bool onboard_mission_available(unsigned relative_index); + + /** + * Check if current mission item has been reached. + */ + bool mission_item_reached(); + + /** + * Move to next waypoint + */ + void advance_mission(); + + /** + * Helper function to get a loiter item + */ + void get_loiter_item(mission_item_s *new_loiter_position); + + /** + * Publish a new mission item triplet for position controller + */ + void publish_mission_item_triplet(); + + /** + * Publish vehicle_control_mode topic for controllers + */ + void publish_control_mode(); + + + /** + * Compare two mission items if they are equivalent + * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. + * + * @return true if equivalent, false otherwise + */ + bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); + + void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -206,39 +323,59 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : + +/* state machine transition table */ + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), + _mavlink_fd(-1), /* subscriptions */ _global_pos_sub(-1), - _att_sub(-1), - _airspeed_sub(-1), + _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _manual_control_sub(-1), + _offboard_mission_sub(-1), + _onboard_mission_sub(-1), + _capabilities_sub(-1), /* publications */ _triplet_pub(-1), + _fence_pub(-1), + _mission_result_pub(-1), + _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + /* states */ - _mission_items_maxcount(20), - _mission_valid(false), - _loiter_hold(false) + _fence_valid(false), + _inside_fence(true), + _mission(), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _set_nav_state_timestamp(0) { - _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); - if (!_mission_items) { - _mission_items_maxcount = 0; - warnx("no free RAM to allocate mission, rejecting any waypoints"); - } + memset(&_fence, 0, sizeof(_fence)); - _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); + _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - /* fetch initial parameter values */ - parameters_update(); + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); + memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); + memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); + + memset(&_mission_result, 0, sizeof(struct mission_result_s)); + + /* Initialize state machine */ + myState = NAV_STATE_INIT; } Navigator::~Navigator() @@ -266,69 +403,87 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -int +void Navigator::parameters_update() { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); - //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); + param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - return OK; + _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } void -Navigator::vehicle_status_poll() +Navigator::global_position_update() { - bool vstatus_updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); - - if (vstatus_updated) { - - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - } + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } void -Navigator::vehicle_attitude_poll() +Navigator::home_position_update() { - /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); +} - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - } +void +Navigator::navigation_capabilities_update() +{ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } + void -Navigator::mission_poll() +Navigator::offboard_mission_update(bool isrotaryWing) { - /* check if there is a new setpoint */ - bool mission_updated; - orb_check(_mission_sub, &mission_updated); + struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ + dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); - if (mission_updated) { + _mission.set_offboard_dataman_id(offboard_mission.dataman_id); + _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); - struct mission_s mission; - orb_copy(ORB_ID(mission), _mission_sub, &mission); + } else { + _mission.set_current_offboard_mission_index(0); + _mission.set_offboard_mission_count(0); + } +} - // XXX this is not optimal yet, but a first prototype / - // test implementation +void +Navigator::onboard_mission_update() +{ + struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - if (mission.count <= _mission_items_maxcount) { - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.set_onboard_mission_count(onboard_mission.count); - memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_valid = true; + } else { + _mission.set_current_onboard_mission_index(0); + _mission.set_onboard_mission_count(0); + } +} - irqrestore(flags); - } else { - warnx("mission larger than storage space"); - } +void +Navigator::vehicle_status_update() +{ + /* try to load initial states */ + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ } } @@ -346,41 +501,60 @@ Navigator::task_main() warnx("Initializing.."); fflush(stdout); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_sub = orb_subscribe(ORB_ID(mission)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + + /* copy all topics first time */ + vehicle_status_update(); + parameters_update(); + global_position_update(); + home_position_update(); + navigation_capabilities_update(); + offboard_mission_update(_vstatus.is_rotary_wing); + onboard_mission_update(); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - parameters_update(); - /* wakeup source(s) */ - struct pollfd fds[2]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + fds[2].fd = _home_pos_sub; + fds[2].events = POLLIN; + fds[3].fd = _capabilities_sub; + fds[3].events = POLLIN; + fds[4].fd = _offboard_mission_sub; + fds[4].events = POLLIN; + fds[5].fd = _onboard_mission_sub; + fds[5].events = POLLIN; + fds[6].fd = _vstatus_sub; + fds[6].events = POLLIN; + while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -390,187 +564,779 @@ Navigator::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + /* only update vehicle status if it changed */ + if (fds[6].revents & POLLIN) { + + vehicle_status_update(); + + /* Evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.main_state == MAIN_STATE_AUTO) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAV_STATE_RTL: + dispatch(EVENT_RTL_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); + } else { + /* try mission, if none is available fallback to loiter instead */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + } + + } else { + /* not in AUTO */ + dispatch(EVENT_NONE_REQUESTED); + } - /* update parameters from storage */ + /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + } + + /* only update parameters if it changed */ + if (fds[0].revents & POLLIN) { parameters_update(); + /* note that these new parameters won't be in effect until a mission triplet is published again */ + } + + /* only update craft capabilities if they have changed */ + if (fds[3].revents & POLLIN) { + navigation_capabilities_update(); + } + + if (fds[4].revents & POLLIN) { + offboard_mission_update(_vstatus.is_rotary_wing); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); + } + + if (fds[5].revents & POLLIN) { + onboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); + } + + if (fds[2].revents & POLLIN) { + home_position_update(); + // XXX check if home position really changed + dispatch(EVENT_HOME_POSITION_CHANGED); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + global_position_update(); + /* only check if waypoint has been reached in Mission or RTL mode */ + if (mission_item_reached()) { + if (_vstatus.main_state == MAIN_STATE_AUTO && + (myState == NAV_STATE_MISSION)) { - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + /* advance by one mission item */ + _mission.move_to_next(); - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } + } else { + dispatch(EVENT_MISSION_FINISHED); + } + } + } - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + publish_control_mode(); - vehicle_attitude_poll(); + perf_end(_loop_perf); + } - mission_poll(); + warnx("exiting."); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); - // Global position - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + _navigator_task = -1; + _exit(0); +} - /* AUTONOMOUS FLIGHT */ +int +Navigator::start() +{ + ASSERT(_navigator_task == -1); - if (1 /* autonomous flight */) { + /* start the task */ + _navigator_task = task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); - /* execute navigation once we have a setpoint */ - if (_mission_valid) { + if (_navigator_task < 0) { + warn("task start failed"); + return -errno; + } - // Next waypoint - math::Vector2f prev_wp; + return OK; +} - if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); +void +Navigator::status() +{ + warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + (double)_global_pos.alt, (double)_global_pos.relative_alt); + warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + } + if (_fence_valid) { + warnx("Geofence is valid"); + warnx("Vertex longitude latitude"); + for (unsigned i = 0; i < _fence.count; i++) + warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { + warnx("Geofence not set"); + } - } else { - /* - * No valid next waypoint, go for heading hold. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + switch (myState) { + case NAV_STATE_INIT: + warnx("State: Init"); + break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + case NAV_STATE_MISSION_LOITER: + warnx("State: Loiter after Mission"); + break; + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + case NAV_STATE_RTL_LOITER: + warnx("State: Loiter after RTL"); + break; + default: + warnx("State: Unknown"); + break; + } +} - } +void +Navigator::publish_fence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} +bool +Navigator::fence_valid(const struct fence_s &fence) +{ + // NULL fence is valid + if (fence.count == 0) { + return true; + } + // Otherwise + if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } - /******** MAIN NAVIGATION STATE MACHINE ********/ + return true; +} - // XXX to be put in its own class +bool +Navigator::load_fence(unsigned vertices) +{ + struct fence_s temp_fence; - if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } - } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + temp_fence.count = i; - /* waypoint is a loiter waypoint */ - - } + if (fence_valid(temp_fence)) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; + return _fence.count != 0; +} - } else { +void +Navigator::fence_point(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publish_fence(0); + return; + } - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt; - _loiter_hold = true; - } + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); - //_parameters.loiter_hold_radius - } + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - } else if (0/* seatbelt mode enabled */) { + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); - /** SEATBELT FLIGHT **/ - continue; + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; - } else { + vertex.lat = (float)lat; + vertex.lon = (float)lon; - /** MANUAL FLIGHT **/ + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publish_fence((unsigned)ix + 1); + return; + } - /* no flight mode applies, do not publish an attitude setpoint */ - continue; - } + errx(1, "can't store fence point"); +} - /******** MAIN NAVIGATION STATE MACHINE ********/ - if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - // XXX define launch position and return - } else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) { - // XXX flared descent on final approach +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { + { + /* STATE_INIT */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, + }, + { + /* STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + }, + { + /* STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + }, + { + /* STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + }, + { + /* STATE_MISSION_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + }, + { + /* STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, + }, + { + /* STATE_RTL_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + }, +}; - } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { +void +Navigator::start_none() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; - /* apply minimum pitch if altitude has not yet been reached */ - if (_global_pos.alt < _global_triplet.current.altitude) { - _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); - } - } + publish_mission_item_triplet(); +} - /* lazily publish the setpoint only once available */ - if (_triplet_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet); +void +Navigator::start_loiter() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet); - } + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? + + get_loiter_item(&_mission_item_triplet.current); + + float global_min_alt = _parameters.min_altitude + _home_pos.altitude; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < global_min_alt) { + _mission_item_triplet.current.altitude = global_min_alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + } else { + _mission_item_triplet.current.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + } + + publish_mission_item_triplet(); +} + + +void +Navigator::start_mission() +{ + /* leave previous mission item as isas is */ + + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_triplet.current_valid = true; + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } + } else { + /* since a mission is started without knowledge if there are more mission items available, this can fail */ + _mission_item_triplet.current_valid = false; + } - perf_end(_loop_perf); + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; } - warnx("exiting.\n"); + publish_mission_item_triplet(); +} - _navigator_task = -1; - _exit(0); + + +void +Navigator::advance_mission() +{ + /* copy current mission to previous item */ + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_triplet.current_valid = true; + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } + } else { + /* since a mission is not advanced without WPs available, this is not supposed to happen */ + _mission_item_triplet.current_valid = false; + warnx("ERROR: current WP can't be set"); + } + + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); + + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; + } + + publish_mission_item_triplet(); } -int -Navigator::start() +void +Navigator::start_mission_loiter() { - ASSERT(_navigator_task == -1); + /* make sure the current WP is valid */ + if (!_mission_item_triplet.current_valid) { + warnx("ERROR: cannot switch to offboard mission loiter"); + } - /* start the task */ - _navigator_task = task_spawn_cmd("navigator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Navigator::task_main_trampoline, - nullptr); + get_loiter_item(&_mission_item_triplet.current); - if (_navigator_task < 0) { - warn("task start failed"); - return -errno; + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); +} + +void +Navigator::start_rtl() +{ + + /* discard all mission item and insert RTL item */ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; + _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + _mission_item_triplet.current_valid = true; + + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); +} + + +void +Navigator::start_rtl_loiter() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; + + get_loiter_item(&_mission_item_triplet.current); + + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); +} + +bool +Navigator::mission_item_reached() +{ + /* only check if there is actually a mission item to check */ + if (!_mission_item_triplet.current_valid) { + return false; } - return OK; + /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + return false; + } + + /* XXX TODO count turns */ + if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item_triplet.current.loiter_radius > 0.01f) { + + return false; + } + + uint64_t now = hrt_absolute_time(); + float orbit; + + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { + + orbit = _mission_item_triplet.current.radius; + + } else { + + // XXX set default orbit via param + orbit = 15.0f; + } + + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + + // TODO add frame + // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); + + // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); + // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); + + // warnx("Dist: %4.4f", dist); + + // } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + // dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); + + // } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { + // dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); + + // } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { + // /* Check if conditions of mission item are satisfied */ + // // XXX TODO + // } + + if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + _waypoint_position_reached = true; + } + + /* check if required yaw reached */ + float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); + float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw); + + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + _waypoint_yaw_reached = true; + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ + + if (_time_first_inside_orbit == 0) { + /* XXX announcment? */ + _time_first_inside_orbit = now; + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) + || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + _time_first_inside_orbit = 0; + _waypoint_yaw_reached = false; + _waypoint_position_reached = false; + return true; + } + } + return false; + +} + +void +Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) +{ + new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_loiter_position->loiter_direction = 1; + new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_loiter_position->radius = 50.0f; // TODO: get rid of magic number + new_loiter_position->autocontinue = false; +} + +void +Navigator::publish_mission_item_triplet() +{ + /* lazily publish the mission triplet only once available */ + if (_triplet_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } +} + +void +Navigator::publish_control_mode() +{ + /* update vehicle_control_mode topic*/ + _control_mode.main_state = _vstatus.main_state; + _control_mode.nav_state = static_cast<nav_state_t>(myState); + _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; + _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; + _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; + + _control_mode.flag_control_offboard_enabled = false; + _control_mode.flag_control_flighttermination_enabled = false; + + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: + break; + } + + _control_mode.timestamp = hrt_absolute_time(); + + /* lazily publish the mission triplet only once available */ + if (_control_mode_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); + + } else { + /* advertise and publish */ + _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); + } +} + +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { + if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && + fabsf(a.lat - b.lat) < FLT_EPSILON && + fabsf(a.lon - b.lon) < FLT_EPSILON && + fabsf(a.altitude - b.altitude) < FLT_EPSILON && + fabsf(a.yaw - b.yaw) < FLT_EPSILON && + fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && + fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && + fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { + return true; + } else { + return false; + } +} + + +static void usage() +{ + errx(1, "usage: navigator {start|stop|status|fence}"); } int navigator_main(int argc, char *argv[]) { - if (argc < 1) - errx(1, "usage: navigator {start|stop|status}"); + if (argc < 2) { + usage(); + } if (!strcmp(argv[1], "start")) { - if (navigator::g_navigator != nullptr) + if (navigator::g_navigator != nullptr) { errx(1, "already running"); + } navigator::g_navigator = new Navigator; - if (navigator::g_navigator == nullptr) + if (navigator::g_navigator == nullptr) { errx(1, "alloc failed"); + } if (OK != navigator::g_navigator->start()) { delete navigator::g_navigator; @@ -578,27 +1344,38 @@ int navigator_main(int argc, char *argv[]) err(1, "start failed"); } - exit(0); + return 0; } - if (!strcmp(argv[1], "stop")) { - if (navigator::g_navigator == nullptr) - errx(1, "not running"); + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + if (!strcmp(argv[1], "stop")) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - exit(0); - } - if (!strcmp(argv[1], "status")) { - if (navigator::g_navigator) { - errx(0, "running"); + } else if (!strcmp(argv[1], "status")) { + navigator::g_navigator->status(); - } else { - errx(1, "not running"); - } + } else if (!strcmp(argv[1], "fence")) { + navigator::g_navigator->fence_point(argc - 2, argv + 2); + + } else { + usage(); } - warnx("unrecognized command"); - return 1; + return 0; +} + +void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp new file mode 100644 index 000000000..6576aae70 --- /dev/null +++ b/src/modules/navigator/navigator_mission.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes <joes@student.ethz.ch> + * + * 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 navigator_mission.cpp + * Helper class to access missions + */ + +// #include <stdio.h> +// #include <stdlib.h> +// #include <string.h> +// #include <unistd.h> + +#include <stdlib.h> +#include <dataman/dataman.h> +#include "navigator_mission.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +Mission::Mission() : + + _offboard_dataman_id(-1), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE) +{} + +Mission::~Mission() +{ + +} + +void +Mission::set_offboard_dataman_id(int new_id) +{ + _offboard_dataman_id = new_id; +} + +void +Mission::set_current_offboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_offboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_offboard_mission_count(unsigned new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(unsigned new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::current_mission_available() +{ + return (current_onboard_mission_available() || current_offboard_mission_available()); + +} + +bool +Mission::next_mission_available() +{ + return (next_onboard_mission_available() || next_offboard_mission_available()); +} + +int +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) +{ + /* try onboard mission first */ + if (current_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { + + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + } else { + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + + return OK; +} + +int +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + /* try onboard mission first */ + if (next_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { + + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; + } + + return OK; +} + + +bool +Mission::current_onboard_mission_available() +{ + return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; +} + +bool +Mission::current_offboard_mission_available() +{ + return _offboard_mission_item_count > _current_offboard_mission_index; +} + +bool +Mission::next_onboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_ONBOARD) { + next = 1; + } + + return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; +} + +bool +Mission::next_offboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_OFFBOARD) { + next = 1; + } + + return _offboard_mission_item_count > (_current_offboard_mission_index + next); +} + +void +Mission::move_to_next() +{ + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + case MISSION_TYPE_NONE: + default: + break; + } +}
\ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h new file mode 100644 index 000000000..15d4e86bf --- /dev/null +++ b/src/modules/navigator/navigator_mission.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes <joes@student.ethz.ch> + * + * 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 navigator_mission.h + * Helper class to access missions + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include <uORB/topics/mission.h> + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor, also kills the sensors task. + */ + ~Mission(); + + void set_offboard_dataman_id(int new_id); + void set_current_offboard_mission_index(int new_index); + void set_current_onboard_mission_index(int new_index); + void set_offboard_mission_count(unsigned new_count); + void set_onboard_mission_count(unsigned new_count); + + void set_onboard_mission_allowed(bool allowed); + + bool current_mission_available(); + bool next_mission_available(); + + int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); + int get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + + void add_home_pos(struct mission_item_s *new_mission_item); + +private: + bool current_onboard_mission_available(); + bool current_offboard_mission_available(); + bool next_onboard_mission_available(); + bool next_offboard_mission_available(); + + int _offboard_dataman_id; + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items available */ + unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; +}; + +#endif
\ No newline at end of file diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 06df9a452..b9d887379 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,7 +1,8 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Author: @autho Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Parameters defined by the navigator task. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #include <nuttx/config.h> @@ -49,5 +51,6 @@ * */ -PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); - +PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); +PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
\ No newline at end of file diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 10007bf96..5bf0fba30 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - errx(1, "subscriptions poll error on init."); + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter > baro_counter) { + if (wait_baro && sensor.baro_counter != baro_counter) { baro_counter = sensor.baro_counter; /* mean calculation over several measurements */ @@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - warnx("subscriptions poll error."); - thread_should_exit = true; + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { @@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accel_counter) { + if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_updates++; } - if (sensor.baro_counter > baro_counter) { + if (sensor.baro_counter != baro_counter) { baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; @@ -615,8 +614,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7d); + global_pos.lon = (int32_t)(est_lon * 1e7d); global_pos.time_gps_usec = gps.time_gps_usec; } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5c621cfb2..541eed0e1 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -50,7 +50,7 @@ #define RC_CHANNEL_HIGH_THRESH 5000 #define RC_CHANNEL_LOW_THRESH -5000 -static bool ppm_input(uint16_t *values, uint16_t *num_values); +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -66,7 +66,7 @@ controls_init(void) sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -94,6 +94,9 @@ controls_tick() { * other. Don't do that. */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ + uint16_t rssi = 0; + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); @@ -104,14 +107,15 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + + rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - r_raw_rc_count = 8; } perf_end(c_gather_sbus); @@ -121,11 +125,20 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + if (ppm_updated) { + + /* XXX sample RSSI properly here */ + rssi = 255; + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + } perf_end(c_gather_ppm); + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -197,14 +210,16 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < PX4IO_CONTROL_CHANNELS); + if (mapped < PX4IO_CONTROL_CHANNELS) { - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + } } } @@ -221,7 +236,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0) { + if (assigned_channels == 0 || rssi == 0) { rc_input_lost = true; } else { /* set RC OK flag */ @@ -306,7 +321,7 @@ controls_tick() { } static bool -ppm_input(uint16_t *values, uint16_t *num_values) +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; @@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > PX4IO_CONTROL_CHANNELS) - *num_values = PX4IO_CONTROL_CHANNELS; + if (*num_values > PX4IO_RC_INPUT_CHANNELS) + *num_values = PX4IO_RC_INPUT_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; @@ -330,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + /* store PPM frame length */ + if (num_values) + *frame_len = ppm_frame_length; + /* good if we got any channels */ result = (*num_values > 0); } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index fd3b72015..4d306d6d0 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) continue; /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) continue; /* update the decoded channel count */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce..e55ef784a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -77,7 +77,8 @@ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -135,10 +136,19 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } @@ -185,7 +195,7 @@ mixer_tick(void) r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -201,6 +211,10 @@ mixer_tick(void) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { @@ -236,24 +250,35 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group > 3) return -1; switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { - control = REG_TO_FLOAT(r_page_controls[control_index]); + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: control = 0.0f; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 5e5396782..e5bef6eb3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -63,7 +63,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -76,6 +76,9 @@ #define PX4IO_PROTOCOL_VERSION 4 +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ @@ -87,6 +90,7 @@ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -124,6 +128,8 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -184,44 +190,59 @@ enum { /* DSM bind states */ dsm_bind_reinit_uart }; /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 52 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ -#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ -#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ -#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ -#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ -#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) -#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* Debug and test page - not used in normal operation */ -#define PX4IO_PAGE_TEST 127 -#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ /* PWM minimum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM maximum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM disarmed values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74..745bd5705 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -45,6 +45,7 @@ #include <string.h> #include <poll.h> #include <signal.h> +#include <crc32.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> @@ -117,6 +118,29 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -129,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. @@ -201,6 +228,7 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -216,6 +244,11 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4fea0288c..dea04a663 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -53,7 +53,9 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels +#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_RC_INPUT_CHANNELS 18 +#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ /* * Debug logging @@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit; #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) + /* * Mixer */ @@ -209,7 +213,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 86a40bc22..0358725db 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -45,6 +45,8 @@ #include <drivers/drv_hrt.h> #include <drivers/drv_pwm_output.h> +#include <systemlib/systemlib.h> +#include <stm32_pwr.h> #include "px4io.h" #include "protocol.h" @@ -68,7 +70,7 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -87,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -112,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -122,7 +126,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -154,6 +158,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_VBATT_SCALE] = 10000, #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, + [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -172,7 +178,7 @@ volatile uint16_t r_page_setup[] = * * Control values from the FMU. */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; /* * PAGE 102 does not have a buffer. @@ -183,7 +189,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -235,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_CONTROLS: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ r_page_controls[offset] = *values; @@ -501,6 +507,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_REBOOT_BL: + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // don't allow reboot while armed + break; + } + + // check the magic value + if (value != PX4IO_REBOOT_BL_MAGIC) + break; + + // note that we don't set BL_WAIT_MAGIC in + // BKP_DR1 as that is not necessary given the + // timing of the forceupdate command. The + // bootloader on px4io waits for enough time + // anyway, and this method works with older + // bootloader versions (tested with both + // revision 3 and revision 4). + + up_systemreset(); + break; + case PX4IO_P_SETUP_DSM: dsm_bind(value & 0x0f, (value >> 4) & 7); break; @@ -525,7 +554,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= PX4IO_CONTROL_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -555,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; + bool disabled = false; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { @@ -573,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } @@ -581,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else { + } else if (!disabled) { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038..cdb54a80a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -77,7 +77,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -86,9 +85,6 @@ safety_init(void) /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -164,16 +160,6 @@ safety_check_button(void *arg) } static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - -static void failsafe_blink(void *arg) { /* indicate that a serious initialisation error occured */ @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -}
\ No newline at end of file +} diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index c523df6ca..11ccd7356 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -54,6 +54,27 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +#define SBUS_FLAGS_BYTE 23 +#define SBUS_FAILSAFE_BIT 3 +#define SBUS_FRAMELOST_BIT 2 + +/* + Measured values with Futaba FX-30/R6108SB: + -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) +*/ + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f + +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) +#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) static int sbus_fd = -1; @@ -66,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, max_channels); + return sbus_decode(now, values, num_values, rssi, max_channels); } /* @@ -194,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint return false; } - /* if the failsafe or connection lost bit is set, we consider the frame invalid */ - if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ - - /* actively announce signal loss */ - *values = 0; - return false; - } - /* we have received something we think is a frame */ last_frame_time = frame_time; @@ -234,22 +246,41 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint } } - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - values[channel] = (value / 2) + 998; + + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ - if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ - values[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - values[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ *num_values = chancount; + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* report that we failed to read anything valid off the receiver */ + *rssi = 0; + return false; + } + else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issueing return-to-launch!!! */ + + *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + } + + *rssi = 255; + return true; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f94875d5b..78322aff3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -68,12 +68,11 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> @@ -691,11 +690,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_global_position_setpoint_s global_pos_sp; + struct mission_item_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -717,11 +715,10 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; - int act_controls_effective_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; - int global_pos_sp_sub; + int triplet_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 20; - /* Sanity check variable and index */ + /* number of subscriptions */ + const ssize_t fdsc = 19; + /* sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.act_controls_effective_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; @@ -849,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - fds[fdsc_count].fd = subs.global_pos_sp_sub; + subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -978,7 +969,8 @@ int sdlog2_thread_main(int argc, char *argv[]) // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + // TODO use control_mode topic + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; @@ -1114,12 +1106,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); - // TODO not implemented yet - } - /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); @@ -1165,20 +1151,21 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; - log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; - log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; - log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; - log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; - log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; - log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; - log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; - log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; - log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; - log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.radius = buf.triplet.current.radius; + log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } @@ -1208,6 +1195,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 90093a407..cb1393f1f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,6 +159,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t channel_count; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -213,13 +214,12 @@ struct log_GPSP_s { int32_t lon; float altitude; float yaw; + uint8_t nav_cmd; float loiter_radius; int8_t loiter_direction; - uint8_t nav_cmd; - float param1; - float param2; - float param3; - float param4; + float radius; + float time_inside; + float pitch_min; }; /* --- ESC - ESC STATE --- */ @@ -281,13 +281,13 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), + LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), + LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b81588..763723554 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -104,49 +104,49 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC9_MIN, 1000); PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); @@ -190,11 +190,30 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC16_MIN, 1000); +PARAM_DEFINE_FLOAT(RC16_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC16_MAX, 2000); +PARAM_DEFINE_FLOAT(RC16_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC17_MIN, 1000); +PARAM_DEFINE_FLOAT(RC17_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC17_MAX, 2000); +PARAM_DEFINE_FLOAT(RC17_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC18_MIN, 1000); +PARAM_DEFINE_FLOAT(RC18_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC18_MAX, 2000); +PARAM_DEFINE_FLOAT(RC18_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else @@ -226,3 +245,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); + +PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec..d9f037c27 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -165,7 +165,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ @@ -260,6 +260,10 @@ private: float rc_scale_yaw; float rc_scale_flaps; + int rc_fs_ch; + int rc_fs_mode; + float rc_fs_thr; + float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -305,6 +309,10 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; + param_t rc_fs_ch; + param_t rc_fs_mode; + param_t rc_fs_thr; + param_t battery_voltage_scaling; param_t board_rotation; @@ -517,6 +525,11 @@ Sensors::Sensors() : _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); + /* RC failsafe */ + _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); + _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); + _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -589,7 +602,7 @@ Sensors::parameters_update() float tmpRevFactor = 0.0f; /* rc values */ - for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { + for (unsigned int i = 0; i < _rc_max_chan_count; i++) { param_get(_parameter_handles.min[i], &(_parameters.min[i])); param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); @@ -668,6 +681,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); + param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); + param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); + param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -1256,6 +1272,18 @@ Sensors::rc_poll() if (rc_input.channel_count < 4) return; + /* failsafe check */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) + return; + + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) + return; + } + } + unsigned channel_limit = rc_input.channel_count; if (channel_limit > _rc_max_chan_count) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index bf84b7945..b4ca0ed3e 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, + pce->time_total / pce->event_count, pce->time_least, pce->time_most); break; diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h index 6c5e15345..5a1ad84da 100644 --- a/src/modules/systemlib/ppm_decode.h +++ b/src/modules/systemlib/ppm_decode.h @@ -57,6 +57,7 @@ __BEGIN_DECLS * PPM decoder state */ __EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */ __EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ __EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b4350cc24..21e15ec56 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -45,7 +45,7 @@ #include <systemlib/rc_check.h> #include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> -#include <uORB/topics/rc_channels.h> +#include <drivers/drv_rc_input.h> int rc_calibration_check(int mavlink_fd) { @@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) { int channel_fail_count = 0; - for (int i = 0; i < RC_CHANNELS_MAX; i++) { + for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { /* should the channel be enabled? */ uint8_t count = 0; diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h new file mode 100644 index 000000000..f2709d29f --- /dev/null +++ b/src/modules/systemlib/state_table.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file state_table.h + * + * Finite-State-Machine helper class for state table + */ + +#ifndef __SYSTEMLIB_STATE_TABLE_H +#define __SYSTEMLIB_STATE_TABLE_H + +class StateTable +{ +public: + typedef void (StateTable::*Action)(); + struct Tran { + Action action; + unsigned nextState; + }; + + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) + : myTable(table), myNsignals(nSignals), myNstates(nStates) {} + + #define NO_ACTION &StateTable::doNothing + #define ACTION(_target) static_cast<StateTable::Action>(_target) + + virtual ~StateTable() {} + + void dispatch(unsigned const sig) { + register Tran const *t = myTable + myState*myNsignals + sig; + (this->*(t->action))(); + + myState = t->nextState; + } + void doNothing() {} +protected: + unsigned myState; +private: + Tran const *myTable; + unsigned myNsignals; + unsigned myNstates; +}; + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3514dca24..79a820c06 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,17 +117,21 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/vehicle_global_position_setpoint.h" -ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); - -#include "topics/vehicle_global_position_set_triplet.h" -ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/mission_item_triplet.h" +ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(onboard_mission, struct mission_s); + +#include "topics/mission_result.h" +ORB_DEFINE(mission_result, struct mission_result_s); + +#include "topics/fence.h" +ORB_DEFINE(fence, unsigned); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); @@ -169,13 +173,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); -/* actuator controls, as set by actuators / mixers after limiting */ -#include "topics/actuator_controls_effective.h" -ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); - #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index d7b404ad4..54d84231f 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -46,34 +46,34 @@ #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#include <stdint.h> -#include "../uORB.h" -#include "actuator_controls.h" +//#include <stdint.h> +//#include "../uORB.h" +//#include "actuator_controls.h" +// +//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +// +///** +// * @addtogroup topics +// * @{ +// */ +// +//struct actuator_controls_effective_s { +// uint64_t timestamp; +// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; +//}; +// +///** +// * @} +// */ +// +///* actuator control sets; this list can be expanded as more controllers emerge */ +//ORB_DECLARE(actuator_controls_effective_0); +//ORB_DECLARE(actuator_controls_effective_1); +//ORB_DECLARE(actuator_controls_effective_2); +//ORB_DECLARE(actuator_controls_effective_3); +// +///* control sets with pre-defined applications */ +//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) -#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_controls_effective_s { - uint64_t timestamp; - float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -}; - -/** - * @} - */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_effective_0); -ORB_DECLARE(actuator_controls_effective_1); -ORB_DECLARE(actuator_controls_effective_2); -ORB_DECLARE(actuator_controls_effective_3); - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/fence.h index a56434d3b..6f16c51cf 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/fence.h @@ -1,9 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Jean Cyr <jean.m.cyr@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,45 +33,41 @@ ****************************************************************************/ /** - * @file vehicle_global_position_setpoint.h - * Definition of the global WGS84 position setpoint uORB topic. + * @file fence.h + * Definition of geofence. */ -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ +#ifndef TOPIC_FENCE_H_ +#define TOPIC_FENCE_H_ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include "mission.h" /** * @addtogroup topics * @{ */ +#define GEOFENCE_MAX_VERTICES 16 + +/** + * This is the position of a geofence vertex + * + */ +struct fence_vertex_s { + // Worst case float precision gives us 2 meter resolution at the equator + float lat; /**< latitude in degrees */ + float lon; /**< longitude in degrees */ +}; + /** - * Global position setpoint in WGS84 coordinates. + * This is the position of a geofence * - * This is the position the MAV is heading towards. If it of type loiter, - * the MAV is circling around it with the given loiter radius in meters. */ -struct vehicle_global_position_setpoint_s -{ - bool altitude_is_relative; /**< true if altitude is relative from start point */ - int32_t lat; /**< latitude in degrees * 1E7 */ - int32_t lon; /**< longitude in degrees * 1E7 */ - float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; - float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ - float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ +struct fence_s { + unsigned count; /**< number of actual vertices */ + struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; /** @@ -81,6 +75,6 @@ struct vehicle_global_position_setpoint_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_setpoint); +ORB_DECLARE(fence); #endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0f..1cf37e29c 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -2,6 +2,7 @@ * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +35,10 @@ /** * @file home_position.h - * Definition of the GPS home position uORB topic. + * Definition of the home position uORB topic. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef TOPIC_HOME_POSITION_H_ @@ -55,16 +57,12 @@ */ struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + + bool altitude_is_relative; + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float altitude; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a..9b4250115 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -35,8 +35,8 @@ ****************************************************************************/ /** - * @file mission_item.h - * Definition of one mission item. + * @file mission.h + * Definition of a mission consisting of mission items. */ #ifndef TOPIC_MISSION_H_ @@ -46,14 +46,24 @@ #include <stdbool.h> #include "../uORB.h" +#define NUM_MISSIONS_SUPPORTED 256 + +/* compatible to mavlink MAV_CMD */ enum NAV_CMD { - NAV_CMD_WAYPOINT = 0, - NAV_CMD_LOITER_TURN_COUNT, - NAV_CMD_LOITER_TIME_LIMIT, - NAV_CMD_LOITER_UNLIMITED, - NAV_CMD_RETURN_TO_LAUNCH, - NAV_CMD_LAND, - NAV_CMD_TAKEOFF + NAV_CMD_WAYPOINT=16, + NAV_CMD_LOITER_UNLIMITED=17, + NAV_CMD_LOITER_TURN_COUNT=18, + NAV_CMD_LOITER_TIME_LIMIT=19, + NAV_CMD_RETURN_TO_LAUNCH=20, + NAV_CMD_LAND=21, + NAV_CMD_TAKEOFF=22, + NAV_CMD_ROI=80, + NAV_CMD_PATHPLANNING=81 +}; + +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD }; /** @@ -70,23 +80,25 @@ enum NAV_CMD { struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ - double lat; /**< latitude in degrees * 1E7 */ - double lon; /**< longitude in degrees * 1E7 */ + double lat; /**< latitude in degrees */ + double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< navigation command */ + float radius; /**< radius in which the mission is accepted as reached in meters */ + float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + bool autocontinue; /**< true if next waypoint should follow after this one */ + enum ORIGIN origin; /**< where the waypoint has been generated */ }; struct mission_s { - struct mission_item_s *items; - unsigned count; + int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the datamanager */ + int current_index; /**< default -1, start at the one changed latest */ }; /** @@ -95,5 +107,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h index 8516b263f..b35eae607 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> @@ -35,18 +35,18 @@ ****************************************************************************/ /** - * @file vehicle_global_position_setpoint.h + * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. */ -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ +#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ +#define TOPIC_MISSION_ITEM_TRIPLET_H_ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include "vehicle_global_position_setpoint.h" +#include "mission.h" /** * @addtogroup topics @@ -58,14 +58,19 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s +struct mission_item_triplet_s { - bool previous_valid; /**< flag indicating previous position is valid */ - bool next_valid; /**< flag indicating next position is valid */ + bool previous_valid; + bool current_valid; /**< flag indicating previous mission item is valid */ + bool next_valid; /**< flag indicating next mission item is valid */ - struct vehicle_global_position_setpoint_s previous; - struct vehicle_global_position_setpoint_s current; - struct vehicle_global_position_setpoint_s next; + struct mission_item_s previous; + struct mission_item_s current; + struct mission_item_s next; + + int previous_index; + int current_index; + int next_index; }; /** @@ -73,6 +78,6 @@ struct vehicle_global_position_set_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_set_triplet); +ORB_DECLARE(mission_item_triplet); #endif diff --git a/src/modules/mavlink/missionlib.h b/src/modules/uORB/topics/mission_result.h index c7d8f90c5..c99706b97 100644 --- a/src/modules/mavlink/missionlib.h +++ b/src/modules/uORB/topics/mission_result.h @@ -1,7 +1,9 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,20 +35,33 @@ ****************************************************************************/ /** - * @file missionlib.h - * MAVLink mission helper library + * @file mission_result.h + * Mission results that navigator needs to pass on to commander and mavlink. */ -#pragma once +#ifndef TOPIC_MISSION_RESULT_H +#define TOPIC_MISSION_RESULT_H -#include "mavlink_bridge_header.h" +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" -//extern void mavlink_wpm_send_message(mavlink_message_t *msg); -//extern void mavlink_wpm_send_gcs_string(const char *string); -//extern uint64_t mavlink_wpm_get_system_timestamp(void); -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); -extern uint64_t mavlink_missionlib_get_system_timestamp(void); -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); +/** + * @addtogroup topics + * @{ + */ + +struct mission_result_s +{ + bool mission_reached; /**< true if mission has been reached */ + unsigned mission_index; /**< index of the mission which has been reached */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(mission_result); + +#endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3..391756f99 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -53,6 +53,11 @@ */ struct navigation_capabilities_s { float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + + /* Landing parameters: see fw_pos_control_l1/landingslope.h */ + float landing_horizontal_slope_displacement; + float landing_slope_angle_rad; + float landing_flare_length; }; /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 5a8580143..086b2ef15 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Nils Wenzler <wenzlern@student.ethz.ch> - * @author Ivan Ovinnikov <oivan@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 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 @@ -47,13 +44,13 @@ /** * The number of RC channel inputs supported. - * Current (Q1/2013) radios support up to 18 channels, + * Current (Q4/2013) radios support up to 18 channels, * leaving at a sane value of 15. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 15 /** * This defines the mapping of the RC functions. @@ -91,7 +88,7 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAX]; + } chan[RC_CHANNELS_MAPPED_MAX]; uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..5ce968f67 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,6 +48,7 @@ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" +#include "vehicle_status.h" /** * @addtogroup topics @{ @@ -59,10 +60,25 @@ * * Encodes the complete system state and is set by the commander app. */ + +typedef enum { + NAV_STATE_INIT = 0, + NAV_STATE_NONE, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_MISSION_LOITER, + NAV_STATE_RTL, + NAV_STATE_RTL_LOITER, + NAV_STATE_MAX +} nav_state_t; + struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + main_state_t main_state; + nav_state_t nav_state; + bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -78,9 +94,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - - bool flag_control_auto_enabled; // TEMP - uint8_t auto_state; // TEMP navigation state for AUTO modes + bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7..ae3a24a1b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,20 +66,6 @@ typedef enum { MAIN_STATE_AUTO, } main_state_t; -/* navigation state machine */ -typedef enum { - NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization - NAVIGATION_STATE_STABILIZE, // attitude stabilization - NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization - NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization - NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff - NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode - NAVIGATION_STATE_AUTO_LOITER, // pause mission - NAVIGATION_STATE_AUTO_MISSION, // fly mission - NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND - NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) -} navigation_state_t; - typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, @@ -96,6 +82,11 @@ typedef enum { } hil_state_t; typedef enum { + FLIGHTTERMINATION_STATE_OFF = 0, + FLIGHTTERMINATION_STATE_ON +} flighttermination_state_t; + +typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_ASSISTED, MODE_SWITCH_AUTO @@ -176,7 +167,8 @@ struct vehicle_status_s uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t navigation_state; /**< navigation state machine */ + unsigned int set_nav_state; /**< set navigation state machine to specified value */ + uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ @@ -229,6 +221,8 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + flighttermination_state_t flighttermination_state; }; /** diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 07581459b..982b03782 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -200,7 +200,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 50; i++) + for (int i = 0; i < 14; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 898c04cd5..7c23f38c2 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -210,7 +210,7 @@ pwm_main(int argc, char *argv[]) err(1, "PWM_SERVO_GET_COUNT"); if (!strcmp(argv[1], "arm")) { - /* tell IO that its ok to disable its safety with the switch */ + /* tell safety that its ok to disable it with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6729ce4ae..68a080c77 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,8 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ - tests_file.c \ + test_file.c \ tests_main.c \ - tests_param.c + test_param.c \ + test_ppm_loopback.c \ + test_rc.c diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c new file mode 100644 index 000000000..5b121e34e --- /dev/null +++ b/src/systemcmds/tests/test_dataman.c @@ -0,0 +1,182 @@ +/**************************************************************************** + * px4/sensors/test_dataman.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <drivers/drv_led.h> +#include <systemlib/systemlib.h> +#include <drivers/drv_hrt.h> +#include <semaphore.h> + + +#include "tests.h" + +#include "dataman/dataman.h" + +static sem_t *sems; + +static int +task_main(int argc, char *argv[]) +{ + char buffer[DM_MAX_DATA_SIZE]; + hrt_abstime wstart, wend, rstart, rend; + + warnx("Starting dataman test task %s", argv[1]); + /* try to read an invalid item */ + int my_id = atoi(argv[1]); + /* try to read an invalid item */ + if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid item failed", my_id); + goto fail; + } + /* try to read an invalid index */ + if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid index failed", my_id); + goto fail; + } + srand(hrt_absolute_time() ^ my_id); + unsigned hit = 0, miss = 0; + wstart = hrt_absolute_time(); + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + memset(buffer, my_id, sizeof(buffer)); + buffer[1] = i; + unsigned hash = i ^ my_id; + unsigned len = (hash & 63) + 2; + + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + warnx("ret: %d", ret); + if (ret != len) { + warnx("%d write failed, index %d, length %d", my_id, hash, len); + goto fail; + } + usleep(rand() & ((64 * 1024) - 1)); + } + rstart = hrt_absolute_time(); + wend = rstart; + + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + unsigned hash = i ^ my_id; + unsigned len2, len = (hash & 63) + 2; + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { + warnx("%d read failed length test, index %d", my_id, hash); + goto fail; + } + if (buffer[0] == my_id) { + hit++; + if (len2 != len) { + warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); + goto fail; + } + if (buffer[1] != i) { + warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); + goto fail; + } + } + else + miss++; + } + rend = hrt_absolute_time(); + warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); + sem_post(sems + my_id); + return 0; +fail: + warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", + my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); + sem_post(sems + my_id); + return -1; +} + +int test_dataman(int argc, char *argv[]) +{ + int i, num_tasks = 4; + char buffer[DM_MAX_DATA_SIZE]; + + if (argc > 1) + num_tasks = atoi(argv[1]); + + sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); + warnx("Running %d tasks", num_tasks); + for (i = 0; i < num_tasks; i++) { + int task; + char a[16]; + sprintf(a, "%d", i); + const char *av[2]; + av[0] = a; + av[1] = 0; + sem_init(sems + i, 1, 0); + /* start the task */ + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { + warn("task start failed"); + } + } + for (i = 0; i < num_tasks; i++) { + sem_wait(sems + i); + sem_destroy(sems + i); + } + free(sems); + dm_restart(DM_INIT_REASON_IN_FLIGHT); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) + break; + } + if (i >= NUM_MISSIONS_SUPPORTED) { + warnx("Restart in-flight failed"); + return -1; + + } + dm_restart(DM_INIT_REASON_POWER_ON); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { + warnx("Restart power-on failed"); + return -1; + } + } + return 0; +} diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/test_file.c index 7f3a654bf..798724cf1 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/test_file.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_file.c + * @file test_file.c * * File write test. */ diff --git a/src/systemcmds/tests/tests_param.c b/src/systemcmds/tests/test_param.c index 13f17bc43..318d2505b 100644 --- a/src/systemcmds/tests/tests_param.c +++ b/src/systemcmds/tests/test_param.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file tests_param.c + * @file test_param.c * * Tests related to the parameter system. */ diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c new file mode 100644 index 000000000..b9041b013 --- /dev/null +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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 test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_rc_input.h> +#include <uORB/topics/rc_channels.h> +#include <systemlib/err.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + +int test_ppm_loopback(int argc, char *argv[]) +{ + + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + + int servo_fd, result; + servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; + servo_position_t pos; + + servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); + + if (servo_fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + } + + printf("Servo readback, pairs of values should match defaults\n"); + + unsigned servo_count; + result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { + result = ioctl(servo_fd, PWM_SERVO_GET(i), (unsigned long)&pos); + + if (result < 0) { + printf("failed reading channel %u\n", i); + } + + //printf("%u: %u %u\n", i, pos, data[i]); + + } + + // /* tell safety that its ok to disable it with the switch */ + // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + // tell output device that the system is armed (it will output values if safety is off) + // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); + // if (result != OK) + // warnx("FAIL: PWM_SERVO_ARM"); + + int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; + + + // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + + // if (result) { + // (void)close(servo_fd); + // return ERROR; + // } else { + // warnx("channel %d set to %d", i, pwm_values[i]); + // } + // } + + warnx("servo count: %d", servo_count); + + struct pwm_output_values pwm_out = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + pwm_out.values[i] = pwm_values[i]; + //warnx("channel %d: disarmed PWM: %d", i+1, pwm_values[i]); + pwm_out.channel_count++; + } + + result = ioctl(servo_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_out); + + /* give driver 10 ms to propagate */ + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); + + /* open PPM input and expect values close to the output values */ + + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + if (rc_updated) { + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + // int ppm_fd = open(RC_INPUT_DEVICE_PATH, O_RDONLY); + + + + // struct rc_input_values rc; + // result = read(ppm_fd, &rc, sizeof(rc)); + + // if (result != sizeof(rc)) { + // warnx("Error reading RC output"); + // (void)close(servo_fd); + // (void)close(ppm_fd); + // return ERROR; + // } + + /* go and check values */ + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + if (fabsf(rc_input.values[i] - pwm_values[i]) > 10) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], pwm_values[i]); + (void)close(servo_fd); + return ERROR; + } + } + } else { + warnx("failed reading RC input data"); + return ERROR; + } + + warnx("PPM LOOPBACK TEST PASSED SUCCESSFULLY!"); + + return 0; +} diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c new file mode 100644 index 000000000..72619fc8b --- /dev/null +++ b/src/systemcmds/tests/test_rc.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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 test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <poll.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_rc_input.h> +#include <systemlib/err.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + +int test_rc(int argc, char *argv[]) +{ + + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + struct rc_input_values rc_last; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); + + /* open PPM input and expect values close to the output values */ + + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + warnx("Reading PPM values - press any key to abort"); + warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); + + if (rc_updated) { + + /* copy initial set */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + rc_last.values[i] = rc_input.values[i]; + } + + rc_last.channel_count = rc_input.channel_count; + + /* poll descriptor */ + struct pollfd fds[2]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + fds[1].fd = 0; + fds[1].events = POLLIN; + + while (true) { + + int ret = poll(fds, 2, 200); + + if (ret > 0) { + + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* go and check values */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); + (void)close(_rc_sub); + return ERROR; + } + + rc_last.values[i] = rc_input.values[i]; + } + + if (rc_last.channel_count != rc_input.channel_count) { + warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); + (void)close(_rc_sub); + return ERROR; + } + + if (hrt_absolute_time() - rc_input.timestamp > 100000) { + warnx("TIMEOUT, less than 10 Hz updates"); + (void)close(_rc_sub); + return ERROR; + } + + } else { + /* key pressed, bye bye */ + return 0; + } + + } + } + + } else { + warnx("failed reading RC input data"); + return ERROR; + } + + warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); + + return 0; +} diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 14503276c..096106ff3 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 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 @@ -49,6 +48,8 @@ #include <fcntl.h> #include <errno.h> #include <debug.h> +#include <math.h> +#include <systemlib/err.h> #include <arch/board/board.h> @@ -77,6 +78,8 @@ static int accel(int argc, char *argv[]); static int gyro(int argc, char *argv[]); static int mag(int argc, char *argv[]); static int baro(int argc, char *argv[]); +static int accel1(int argc, char *argv[]); +static int gyro1(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -91,6 +94,8 @@ struct { {"gyro", "/dev/gyro", gyro}, {"mag", "/dev/mag", mag}, {"baro", "/dev/baro", baro}, + {"accel1", "/dev/accel1", accel1}, + {"gyro1", "/dev/gyro1", gyro1}, {NULL, NULL, NULL} }; @@ -133,23 +138,58 @@ accel(int argc, char *argv[]) printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } - // /* wait at least 10ms, sensor should have data after no more than 2ms */ - // usleep(100000); + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("ACCEL1 acceleration values out of range!"); + return ERROR; + } + + /* Let user know everything is ok */ + printf("\tOK: ACCEL passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +accel1(int argc, char *argv[]) +{ + printf("\tACCEL1: test start\n"); + fflush(stdout); + + int fd; + struct accel_report buf; + int ret; - // ret = read(fd, buf, sizeof(buf)); + fd = open("/dev/accel1", O_RDONLY); - // if (ret != sizeof(buf)) { - // printf("\tMPU-6000: read2 fail (%d)\n", ret); - // return ERROR; + if (fd < 0) { + printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n"); + return ERROR; + } + + /* wait at least 100ms, sensor should have data after no more than 20ms */ + usleep(100000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tACCEL1: read1 fail (%d)\n", ret); + return ERROR; - // } else { - // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); - // } + } else { + printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); + } - /* XXX more tests here */ + if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { + warnx("ACCEL1 acceleration values out of range!"); + return ERROR; + } /* Let user know everything is ok */ - printf("\tOK: ACCEL passed all tests successfully\n"); + printf("\tOK: ACCEL1 passed all tests successfully\n"); + + close(fd); return OK; } @@ -187,6 +227,45 @@ gyro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +gyro1(int argc, char *argv[]) +{ + printf("\tGYRO1: test start\n"); + fflush(stdout); + + int fd; + struct gyro_report buf; + int ret; + + fd = open("/dev/gyro1", O_RDONLY); + + if (fd < 0) { + printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tGYRO1: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: GYRO1 passed all tests successfully\n"); + close(fd); return OK; } @@ -224,6 +303,7 @@ mag(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); + close(fd); return OK; } @@ -261,6 +341,7 @@ baro(int argc, char *argv[]) /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); + close(fd); return OK; } diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index f95760ca8..ef8512bf5 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -1,7 +1,6 @@ /**************************************************************************** - * px4/sensors/test_hrt.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -32,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_servo.c + * Tests the servo outputs + * + */ #include <nuttx/config.h> @@ -55,39 +56,6 @@ #include "tests.h" -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_servo - ****************************************************************************/ - int test_servo(int argc, char *argv[]) { int fd, result; @@ -110,7 +78,14 @@ int test_servo(int argc, char *argv[]) printf("Servo readback, pairs of values should match defaults\n"); - for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { + unsigned servo_count; + result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { + warnx("PWM_SERVO_GET_COUNT"); + return ERROR; + } + + for (unsigned i = 0; i < servo_count; i++) { result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); if (result < 0) { @@ -122,11 +97,20 @@ int test_servo(int argc, char *argv[]) } - printf("Servos arming at default values\n"); + /* tell safety that its ok to disable it with the switch */ + result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); + if (result != OK) + warnx("FAIL: PWM_SERVO_ARM"); + usleep(5000000); printf("Advancing channel 0 to 1500\n"); result = ioctl(fd, PWM_SERVO_SET(0), 1500); + printf("Advancing channel 1 to 1800\n"); + result = ioctl(fd, PWM_SERVO_SET(1), 1800); out: return 0; } diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 3be152004..f1d41739b 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -1,8 +1,6 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 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 @@ -14,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -33,9 +31,11 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_uart_loopback.c + * Tests the uart outputs + * + */ #include <nuttx/config.h> @@ -55,40 +55,6 @@ #include <math.h> #include <float.h> - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_led - ****************************************************************************/ - int test_uart_loopback(int argc, char *argv[]) { @@ -97,11 +63,11 @@ int test_uart_loopback(int argc, char *argv[]) int uart5_nwrite = 0; int uart2_nwrite = 0; - int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); // + /* opening stdout */ + int stdout_fd = 1; - /* assuming NuttShell is on UART1 (/dev/ttyS0) */ - int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // - int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); // + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); + int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); if (uart2 < 0) { printf("ERROR opening UART2, aborting..\n"); @@ -113,7 +79,7 @@ int test_uart_loopback(int argc, char *argv[]) exit(uart5); } - uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; + uint8_t sample_stdout_fd[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; @@ -121,7 +87,7 @@ int test_uart_loopback(int argc, char *argv[]) for (i = 0; i < 1000; i++) { // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); @@ -130,7 +96,7 @@ int test_uart_loopback(int argc, char *argv[]) uart2_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* uart2 -> uart5 */ r = write(uart5, sample_uart5, sizeof(sample_uart5)); @@ -139,7 +105,7 @@ int test_uart_loopback(int argc, char *argv[]) uart5_nwrite += r; // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); /* try to read back values */ do { @@ -150,7 +116,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); - write(uart1, sample_uart1, sizeof(sample_uart1)); + write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); do { r = read(uart2, sample_uart5, sizeof(sample_uart5)); @@ -160,7 +126,7 @@ int test_uart_loopback(int argc, char *argv[]) } while (r > 0); // printf("TEST #%d\n",i); -// write(uart1, sample_uart1, sizeof(sample_uart5)); +// write(stdout_fd, sample_stdout_fd, sizeof(sample_uart5)); } for (i = 0; i < 200000; i++) { @@ -181,7 +147,7 @@ int test_uart_loopback(int argc, char *argv[]) } - close(uart1); + close(stdout_fd); close(uart2); close(uart5); diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf..a57d04be3 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -34,6 +34,12 @@ #ifndef __APPS_PX4_TESTS_H #define __APPS_PX4_TESTS_H +/** + * @file tests.h + * Tests declaration file. + * + */ + /**************************************************************************** * Included Files ****************************************************************************/ @@ -88,6 +94,7 @@ extern int test_int(int argc, char *argv[]); extern int test_float(int argc, char *argv[]); extern int test_ppm(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); +extern int test_ppm_loopback(int argc, char *argv[]); extern int test_uart_loopback(int argc, char *argv[]); extern int test_uart_baudchange(int argc, char *argv[]); extern int test_cpuload(int argc, char *argv[]); @@ -101,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_rc(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9eb9c632e..1088a4407 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 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 @@ -35,6 +34,8 @@ /** * @file tests_main.c * Tests main file, loads individual tests. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> @@ -58,14 +59,6 @@ #include "tests.h" /**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** * Private Function Prototypes ****************************************************************************/ @@ -94,6 +87,7 @@ const struct { {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST}, {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST}, {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"ppm_loopback", test_ppm_loopback, OPT_NOALLTEST}, {"adc", test_adc, OPT_NOJIGTEST}, {"jig_voltages", test_jig_voltages, OPT_NOALLTEST}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, @@ -111,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |