From 9defc6cb235e13ef912a70466acf1d14314f1e3d Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 14:26:17 +0100 Subject: [PATCH 01/10] mkblctrl fmuv2 support added --- makefiles/config_px4fmu-v2_default.mk | 2 + src/drivers/mkblctrl/mkblctrl.cpp | 152 +++++++++++++++++++++++++- 2 files changed, 148 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 880e2738ac..dc9208339b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -38,6 +38,8 @@ MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors +MODULES += drivers/mkblctrl + # Needs to be burned to the ground and re-written; for now, # just don't build it. diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 30d6069b32..dbba91786b 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * Author: Marco Bauer * * Redistribution and use in source and binary forms, with or without @@ -93,6 +93,9 @@ #define MOTOR_SPINUP_COUNTER 30 #define ESC_UORB_PUBLISH_DELAY 500000 + + + class MK : public device::I2C { public: @@ -181,6 +184,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); @@ -196,6 +200,7 @@ private: }; const MK::GPIOConfig MK::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -204,6 +209,22 @@ const MK::GPIOConfig MK::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, +#endif }; const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); @@ -623,9 +644,11 @@ MK::task_main() if(!_overrideSecurityChecks) { /* don't go under BLCTRL_MIN_VALUE */ + if (outputs.output[i] < BLCTRL_MIN_VALUE) { outputs.output[i] = BLCTRL_MIN_VALUE; } + } /* output to BLCtrl's */ @@ -1075,6 +1098,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; @@ -1198,23 +1225,115 @@ MK::write(file *filp, const char *buffer, size_t len) return count * 2; } +void +MK::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_configgpio(GPIO_SPI_CS_MPU_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_gpiowrite(GPIO_SPI_CS_MPU_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_configgpio(GPIO_EXTI_MPU_DRDY_OFF); + + stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_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); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + +#endif +#endif +} + + void MK::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + 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); + } + } + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); +#endif } void MK::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. @@ -1227,6 +1346,8 @@ MK::gpio_set_function(uint32_t gpios, int function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } +#endif + /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { if (gpios & (1 << i)) { @@ -1248,9 +1369,13 @@ MK::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 } void @@ -1418,6 +1543,20 @@ mk_start(unsigned bus, unsigned motors, char *device_path) return ret; } +void +sensor_reset(int ms) +{ + int fd; + + 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"); + +} int mk_check_for_i2c_esc_bus(char *device_path, int motors) @@ -1426,6 +1565,7 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) if (g_mk == nullptr) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) g_mk = new MK(3, device_path); if (g_mk == nullptr) { @@ -1441,7 +1581,7 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } } - +#endif g_mk = new MK(1, device_path); From 0089db7ef3961c36d6513877b7681ab548d20ccf Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 16:28:56 +0100 Subject: [PATCH 02/10] code cleanup --- src/drivers/mkblctrl/mkblctrl.cpp | 425 +++--------------------------- 1 file changed, 30 insertions(+), 395 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index dbba91786b..c3c4bf8c1d 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -65,7 +65,7 @@ #include #include -#include +//#include #include #include @@ -123,8 +123,7 @@ public: virtual int init(unsigned motors); virtual ssize_t write(file *filp, const char *buffer, size_t len); - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); + int set_update_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); @@ -136,7 +135,6 @@ private: static const unsigned _max_actuators = MAX_MOTORS; static const bool showDebug = false; - Mode _mode; int _update_rate; int _current_update_rate; int _task; @@ -183,51 +181,15 @@ private: static const GPIOConfig _gpio_tab[]; 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); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, short val); int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); short scaling(float val, float inMin, float inMax, float outMin, float outMax); - - }; -const MK::GPIOConfig MK::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {0, GPIO_VDD_5V_PERIPH_EN, 0}, - {0, GPIO_VDD_3V3_SENSORS_EN, 0}, - {GPIO_VDD_BRICK_VALID, 0, 0}, - {GPIO_VDD_SERVO_VALID, 0, 0}, - {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, - {GPIO_VDD_5V_PERIPH_OC, 0, 0}, -#endif -}; -const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration @@ -268,8 +230,7 @@ MK *g_mk; MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), - _mode(MODE_NONE), - _update_rate(50), + _update_rate(400), _task(-1), _t_actuators(-1), _t_actuator_armed(-1), @@ -348,9 +309,6 @@ MK::init(unsigned motors) } - /* reset GPIOs */ - gpio_reset(); - /* start the IO interface task */ _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, @@ -375,43 +333,7 @@ MK::task_main_trampoline(int argc, char *argv[]) } int -MK::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_4PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -MK::set_pwm_rate(unsigned rate) +MK::set_update_rate(unsigned rate) { if ((rate > 500) || (rate < 10)) return -EINVAL; @@ -1042,28 +964,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - - /* try it as a GPIO ioctl first */ - ret = gpio_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) - return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - /* - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_6PWM: - ret = pwm_ioctl(filp, cmd, arg); - break; - - default: - debug("not in a PWM mode"); - break; - } - */ ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ @@ -1099,7 +999,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = 400; + *(uint32_t *)arg = _update_rate; break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: @@ -1225,237 +1125,10 @@ MK::write(file *filp, const char *buffer, size_t len) return count * 2; } -void -MK::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_configgpio(GPIO_SPI_CS_MPU_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_gpiowrite(GPIO_SPI_CS_MPU_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_configgpio(GPIO_EXTI_MPU_DRDY_OFF); - - stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); - stm32_gpiowrite(GPIO_EXTI_MPU_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); - - // // XXX bring up the EXTI pins again - // stm32_configgpio(GPIO_GYRO_DRDY); - // stm32_configgpio(GPIO_MAG_DRDY); - // stm32_configgpio(GPIO_ACCEL_DRDY); - // stm32_configgpio(GPIO_EXTI_MPU_DRDY); - -#endif -#endif -} - - -void -MK::gpio_reset(void) -{ - /* - * Setup default GPIO config - all pins as GPIOs, input if - * possible otherwise output if possible. - */ - 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); - } - } - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - /* if we have a GPIO direction control, set it to zero (input) */ - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); -#endif -} - -void -MK::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. - */ - if (gpios & 3) { - gpios |= 3; - - /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) - stm32_gpiowrite(GPIO_GPIO_DIR, 1); - } - -#endif - - /* configure selected GPIOs as required */ - for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1 << i)) { - switch (function) { - case GPIO_SET_INPUT: - stm32_configgpio(_gpio_tab[i].input); - break; - - case GPIO_SET_OUTPUT: - stm32_configgpio(_gpio_tab[i].output); - break; - - case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) - stm32_configgpio(_gpio_tab[i].alt); - - break; - } - } - } - -#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 -} - -void -MK::gpio_write(uint32_t gpios, int function) -{ - int value = (function == GPIO_SET) ? 1 : 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) - stm32_gpiowrite(_gpio_tab[i].output, value); -} - -uint32_t -MK::gpio_read(void) -{ - uint32_t bits = 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) - bits |= (1 << i); - - return bits; -} - -int -MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - - case GPIO_RESET: - gpio_reset(); - break; - - case GPIO_SET_OUTPUT: - case GPIO_SET_INPUT: - case GPIO_SET_ALT_1: - gpio_set_function(arg, cmd); - break; - - case GPIO_SET_ALT_2: - case GPIO_SET_ALT_3: - case GPIO_SET_ALT_4: - ret = -EINVAL; - break; - - case GPIO_SET: - case GPIO_CLEAR: - gpio_write(arg, cmd); - break; - - case GPIO_GET: - *(uint32_t *)arg = gpio_read(); - break; - - default: - ret = -ENOTTY; - } - - unlock(); - - return ret; -} namespace { -enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_SERIAL, - PORT_FULL_PWM, - PORT_GPIO_AND_SERIAL, - PORT_PWM_AND_SERIAL, - PORT_PWM_AND_GPIO, -}; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -1466,20 +1139,11 @@ enum FrameType { FRAME_X, }; -PortMode g_port_mode; int -mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) +mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { - uint32_t gpio_bits; int shouldStop = 0; - MK::Mode servo_mode; - - /* reset to all-inputs */ - g_mk->ioctl(0, GPIO_RESET, 0); - - gpio_bits = 0; - servo_mode = MK::MODE_NONE; /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); @@ -1493,7 +1157,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* ovveride security checks if enabled */ g_mk->set_overrideSecurityChecks(overrideSecurityChecks); - /* count used motors */ do { if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { @@ -1508,12 +1171,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, 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); - - - if ((servo_mode != MK::MODE_NONE) && (update_rate != 0)) - g_mk->set_pwm_rate(update_rate); + g_mk->set_update_rate(update_rate); return OK; } @@ -1543,60 +1201,38 @@ mk_start(unsigned bus, unsigned motors, char *device_path) return ret; } -void -sensor_reset(int ms) -{ - int fd; - - 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"); - -} int mk_check_for_i2c_esc_bus(char *device_path, int motors) { int ret; + g_mk = new MK(1, device_path); + if (g_mk == nullptr) { - -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - 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; - } - + return -1; + } else if (OK != g_mk) { + delete g_mk; + g_mk = nullptr; + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + if (ret > 0) { + return 3; } -#endif + } - 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; - } + 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; } } @@ -1612,7 +1248,6 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); int 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; @@ -1729,7 +1364,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); } exit(0); From 816229652f1eecf8322603eb918f787bdd77d7e2 Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 2 Feb 2014 20:36:11 +0100 Subject: [PATCH 03/10] i2c1 bug and bus scan fixed --- src/drivers/mkblctrl/mkblctrl.cpp | 62 +++++++++++++++---------------- 1 file changed, 29 insertions(+), 33 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c3c4bf8c1d..f692a0dd0d 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -65,7 +65,6 @@ #include #include -//#include #include #include @@ -99,13 +98,6 @@ class MK : public device::I2C { public: - enum Mode { - MODE_NONE, - MODE_2PWM, - MODE_4PWM, - MODE_6PWM, - }; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -1207,11 +1199,11 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) { int ret; - g_mk = new MK(1, device_path); + // try bus 3 first + warnx("scanning i2c3..."); + g_mk = new MK(3, device_path); - if (g_mk == nullptr) { - return -1; - } else if (OK != g_mk) { + if (g_mk != nullptr && OK != g_mk->init(motors)) { delete g_mk; g_mk = nullptr; } else { @@ -1223,10 +1215,13 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } } + // fallback to bus 1 + warnx("scanning i2c1..."); g_mk = new MK(1, device_path); - if (g_mk == nullptr) { - return -1; + if (g_mk != nullptr && OK != g_mk->init(motors)) { + delete g_mk; + g_mk = nullptr; } else { ret = g_mk->mk_check_for_blctrl(8, false, true); delete g_mk; @@ -1240,7 +1235,6 @@ mk_check_for_i2c_esc_bus(char *device_path, int motors) } - } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1348,31 +1342,33 @@ mkblctrl_main(int argc, char *argv[]) if (!motortest) { - if (g_mk == nullptr) { - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - if (bus != -1) { + + } + + 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)"); - } + } 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(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } - exit(0); - } else { - errx(1, "MK-BLCtrl driver already running"); - } + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } - } else { + } else { if (g_mk == nullptr) { errx(1, "MK-BLCtrl driver not running. You have to start it first."); From a7218770b3cb6c45927d5c791aa863ffccba6b89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Feb 2014 15:33:56 +0100 Subject: [PATCH 04/10] Updated MAVLink version, no functional changes --- .../v1.0/ardupilotmega/ardupilotmega.h | 7 +- .../v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 287 ++++++++++++ .../mavlink/v1.0/ardupilotmega/testsuite.h | 54 +++ .../mavlink/v1.0/ardupilotmega/version.h | 4 +- .../include/mavlink/v1.0/autoquad/autoquad.h | 6 +- .../include/mavlink/v1.0/autoquad/version.h | 4 +- mavlink/include/mavlink/v1.0/common/common.h | 10 +- .../mavlink_msg_data_transmission_handshake.h | 68 +-- .../mavlink_msg_encapsulated_data.h | 6 +- .../v1.0/common/mavlink_msg_gps2_raw.h | 419 ++++++++++++++++++ .../v1.0/common/mavlink_msg_gps_inject_data.h | 237 ++++++++++ .../include/mavlink/v1.0/common/testsuite.h | 218 +++++++++ mavlink/include/mavlink/v1.0/common/version.h | 4 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 6 +- .../mavlink/v1.0/matrixpilot/version.h | 4 +- mavlink/include/mavlink/v1.0/mavlink_types.h | 2 +- .../include/mavlink/v1.0/pixhawk/pixhawk.h | 8 +- .../include/mavlink/v1.0/pixhawk/testsuite.h | 102 ----- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- .../mavlink/v1.0/sensesoar/sensesoar.h | 6 +- .../mavlink/v1.0/sensesoar/testsuite.h | 26 +- .../include/mavlink/v1.0/sensesoar/version.h | 4 +- 22 files changed, 1300 insertions(+), 184 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h rename mavlink/include/mavlink/v1.0/{pixhawk => common}/mavlink_msg_data_transmission_handshake.h (89%) rename mavlink/include/mavlink/v1.0/{pixhawk => common}/mavlink_msg_encapsulated_data.h (98%) create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 4dc9aed260..2acb7965ca 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -197,6 +197,7 @@ enum RALLY_FLAGS #include "./mavlink_msg_airspeed_autocal.h" #include "./mavlink_msg_rally_point.h" #include "./mavlink_msg_rally_fetch_point.h" +#include "./mavlink_msg_ahrs2.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h new file mode 100644 index 0000000000..f6fde9590c --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h @@ -0,0 +1,287 @@ +// MESSAGE AHRS2 PACKING + +#define MAVLINK_MSG_ID_AHRS2 178 + +typedef struct __mavlink_ahrs2_t +{ + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float altitude; ///< Altitude (MSL) + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 +} mavlink_ahrs2_t; + +#define MAVLINK_MSG_ID_AHRS2_LEN 24 +#define MAVLINK_MSG_ID_178_LEN 24 + +#define MAVLINK_MSG_ID_AHRS2_CRC 47 +#define MAVLINK_MSG_ID_178_CRC 47 + + + +#define MAVLINK_MESSAGE_INFO_AHRS2 { \ + "AHRS2", \ + 6, \ + { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \ + { "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \ + } \ +} + + +/** + * @brief Pack a ahrs2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Pack a ahrs2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN); +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} + +/** + * @brief Encode a ahrs2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Encode a ahrs2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2) +{ + return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng); +} + +/** + * @brief Send a ahrs2 message + * @param chan MAVLink channel to send the message + * + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param altitude Altitude (MSL) + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AHRS2_LEN]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.altitude = altitude; + packet.lat = lat; + packet.lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AHRS2 UNPACKING + + +/** + * @brief Get field roll from ahrs2 message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field pitch from ahrs2 message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from ahrs2 message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude from ahrs2 message + * + * @return Altitude (MSL) + */ +static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field lat from ahrs2 message + * + * @return Latitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lng from ahrs2 message + * + * @return Longitude in degrees * 1E7 + */ +static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Decode a ahrs2 message into a struct + * + * @param msg The message to decode + * @param ahrs2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg); + ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg); + ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg); + ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg); + ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg); + ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg); +#else + memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index eb3bc6a759..b2496334f7 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_ahrs2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ahrs2_t packet_in = { + 17.0, + }45.0, + }73.0, + }101.0, + }963498296, + }963498504, + }; + mavlink_ahrs2_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.altitude = packet_in.altitude; + packet1.lat = packet_in.lat; + packet1.lng = packet_in.lng; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng ); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ahrs2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng ); + mavlink_msg_ahrs2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; isize = mavlink_msg_data_transmission_handshake_get_size(msg); data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg); data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg); - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); + data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); #else diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h similarity index 98% rename from mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h rename to mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h index 40001dc309..5900ea8388 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h @@ -1,6 +1,6 @@ // MESSAGE ENCAPSULATED_DATA PACKING -#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131 typedef struct __mavlink_encapsulated_data_t { @@ -9,10 +9,10 @@ typedef struct __mavlink_encapsulated_data_t } mavlink_encapsulated_data_t; #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 -#define MAVLINK_MSG_ID_194_LEN 255 +#define MAVLINK_MSG_ID_131_LEN 255 #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223 -#define MAVLINK_MSG_ID_194_CRC 223 +#define MAVLINK_MSG_ID_131_CRC 223 #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h new file mode 100644 index 0000000000..17e5bd0026 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -0,0 +1,419 @@ +// MESSAGE GPS2_RAW PACKING + +#define MAVLINK_MSG_ID_GPS2_RAW 124 + +typedef struct __mavlink_gps2_raw_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) + uint32_t dgps_age; ///< Age of DGPS info + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 + uint8_t dgps_numch; ///< Number of DGPS satellites +} mavlink_gps2_raw_t; + +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 +#define MAVLINK_MSG_ID_124_LEN 35 + +#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 +#define MAVLINK_MSG_ID_124_CRC 87 + + + +#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ + "GPS2_RAW", \ + 12, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \ + { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ + { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ + } \ +} + + +/** + * @brief Pack a gps2_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Pack a gps2_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + +/** + * @brief Encode a gps2_raw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Encode a gps2_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); +} + +/** + * @brief Send a gps2_raw message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param dgps_numch Number of DGPS satellites + * @param dgps_age Age of DGPS info + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} + +#endif + +// MESSAGE GPS2_RAW UNPACKING + + +/** + * @brief Get field time_usec from gps2_raw message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps2_raw message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field lat from gps2_raw message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from gps2_raw message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from gps2_raw message + * + * @return Altitude (WGS84), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from gps2_raw message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field epv from gps2_raw message + * + * @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field vel from gps2_raw message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field cog from gps2_raw message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + */ +static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field satellites_visible from gps2_raw message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field dgps_numch from gps2_raw message + * + * @return Number of DGPS satellites + */ +static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field dgps_age from gps2_raw message + * + * @return Age of DGPS info + */ +static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Decode a gps2_raw message into a struct + * + * @param msg The message to decode + * @param gps2_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg); + gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg); + gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg); + gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg); + gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg); + gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg); + gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg); + gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg); + gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg); + gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); + gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); + gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); +#else + memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h new file mode 100644 index 0000000000..485d8a4afc --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h @@ -0,0 +1,237 @@ +// MESSAGE GPS_INJECT_DATA PACKING + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123 + +typedef struct __mavlink_gps_inject_data_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t len; ///< data length + uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2) +} mavlink_gps_inject_data_t; + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113 +#define MAVLINK_MSG_ID_123_LEN 113 + +#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250 +#define MAVLINK_MSG_ID_123_CRC 250 + +#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110 + +#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \ + "GPS_INJECT_DATA", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \ + } \ +} + + +/** + * @brief Pack a gps_inject_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Pack a gps_inject_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t len,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + +/** + * @brief Encode a gps_inject_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Encode a gps_inject_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + +/** + * @brief Send a gps_inject_data message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param len data length + * @param data raw data (110 is enough for 12 satellites of RTCMv2) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} + +#endif + +// MESSAGE GPS_INJECT_DATA UNPACKING + + +/** + * @brief Get field target_system from gps_inject_data message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from gps_inject_data message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field len from gps_inject_data message + * + * @return data length + */ +static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field data from gps_inject_data message + * + * @return raw data (110 is enough for 12 satellites of RTCMv2) + */ +static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 110, 3); +} + +/** + * @brief Decode a gps_inject_data message into a struct + * + * @param msg The message to decode + * @param gps_inject_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg); + gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg); + gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg); + mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data); +#else + memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index 16250ab427..c5aa9ddf3a 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -4839,6 +4839,220 @@ static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_gps_inject_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_inject_data_t packet_in = { + 5, + }72, + }139, + }{ 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59 }, + }; + mavlink_gps_inject_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.len = packet_in.len; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*110); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data ); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data ); + mavlink_msg_gps_inject_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i Date: Tue, 4 Feb 2014 16:50:22 +0100 Subject: [PATCH 05/10] startup rewrite --- src/drivers/mkblctrl/mkblctrl.cpp | 108 ++++++++---------------------- 1 file changed, 27 insertions(+), 81 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index f692a0dd0d..d1c817cf31 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1169,69 +1169,39 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned bus, unsigned motors, char *device_path) -{ - int ret = OK; - - if (g_mk == nullptr) { - - g_mk = new MK(bus, device_path); - - if (g_mk == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_mk->init(motors); - - if (ret != OK) { - delete g_mk; - g_mk = nullptr; - } - } - } - - return ret; -} - - -int -mk_check_for_i2c_esc_bus(char *device_path, int motors) +mk_start(unsigned motors, char *device_path) { int ret; - // try bus 3 first - warnx("scanning i2c3..."); - g_mk = new MK(3, device_path); + // try i2c3 first + g_mk = new MK(3, device_path); - if (g_mk != nullptr && OK != g_mk->init(motors)) { - delete g_mk; - g_mk = nullptr; - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - if (ret > 0) { - return 3; - } - } + if (g_mk && OK == g_mk->init(motors)) { + fprintf(stderr, "[mkblctrl] scanning i2c3...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; + } + } + + delete g_mk; + g_mk = nullptr; // fallback to bus 1 - warnx("scanning i2c1..."); g_mk = new MK(1, device_path); - if (g_mk != nullptr && OK != g_mk->init(motors)) { - delete g_mk; - g_mk = nullptr; - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - if (ret > 0) { - return 1; - } - } + if (g_mk && OK == g_mk->init(motors)) { + fprintf(stderr, "[mkblctrl] scanning i2c1...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; + } + } - return -1; + delete g_mk; + g_mk = nullptr; + + return -ENOMEM; } @@ -1244,7 +1214,6 @@ mkblctrl_main(int argc, char *argv[]) { int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1258,18 +1227,6 @@ mkblctrl_main(int argc, char *argv[]) */ for (int i = 1; i < argc; i++) { - /* look for the optional i2c bus parameter */ - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { - if (argc > i + 1) { - bus = atoi(argv[i + 1]); - newMode = true; - - } else { - errx(1, "missing argument for i2c bus (-b)"); - return 1; - } - } - /* look for the optional frame parameter */ if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { if (argc > i + 1) { @@ -1329,7 +1286,6 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, "mkblctrl: help:\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"); @@ -1343,19 +1299,9 @@ mkblctrl_main(int argc, char *argv[]) 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)"); - } + if (mk_start(motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } /* parameter set ? */ if (newMode) { From 94b162d0e076a872af9d1b1538d7f688d51bfef0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 09:34:21 +0100 Subject: [PATCH 06/10] Fixed up nullptr handling --- src/drivers/mkblctrl/mkblctrl.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index d1c817cf31..46f7905fff 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1176,8 +1176,11 @@ mk_start(unsigned motors, char *device_path) // try i2c3 first g_mk = new MK(3, device_path); - if (g_mk && OK == g_mk->init(motors)) { - fprintf(stderr, "[mkblctrl] scanning i2c3...\n"); + if (!g_mk) + return -ENOMEM; + + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c3...\n"); ret = g_mk->mk_check_for_blctrl(8, false, true); if (ret > 0) { return OK; @@ -1190,8 +1193,11 @@ mk_start(unsigned motors, char *device_path) // fallback to bus 1 g_mk = new MK(1, device_path); - if (g_mk && OK == g_mk->init(motors)) { - fprintf(stderr, "[mkblctrl] scanning i2c1...\n"); + if (!g_mk) + return -ENOMEM; + + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c1...\n"); ret = g_mk->mk_check_for_blctrl(8, false, true); if (ret > 0) { return OK; @@ -1201,7 +1207,7 @@ mk_start(unsigned motors, char *device_path) delete g_mk; g_mk = nullptr; - return -ENOMEM; + return -ENXIO; } From 399d59483ede8a6c7c66c3d56f3025e1650d665e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 09:36:22 +0100 Subject: [PATCH 07/10] Fixed code style --- src/drivers/mkblctrl/mkblctrl.cpp | 107 ++++++++++++++++-------------- 1 file changed, 57 insertions(+), 50 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 46f7905fff..ec5f77d747 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -291,23 +291,23 @@ MK::init(unsigned motors) usleep(500000); - if (sizeof(_device) > 0) { - ret = register_driver(_device, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { + if (ret == OK) { log("creating alternate output device"); _primary_pwm_device = true; } - } + } /* start the IO interface task */ _task = task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { @@ -556,7 +556,7 @@ MK::task_main() } } - if(!_overrideSecurityChecks) { + if (!_overrideSecurityChecks) { /* don't go under BLCTRL_MIN_VALUE */ if (outputs.output[i] < BLCTRL_MIN_VALUE) { @@ -612,21 +612,24 @@ MK::task_main() esc.esc[i].esc_current = (uint16_t) Motor[i].Current; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; + if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) - esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits; + esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } + 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); - } + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } } @@ -665,7 +668,7 @@ MK::mk_servo_arm(bool status) unsigned int MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { - if(initI2C) { + if (initI2C) { I2C::init(); } @@ -718,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - - if(!_overrideSecurityChecks) { + + if (!_overrideSecurityChecks) { if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { _task_should_exit = true; } @@ -748,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val) tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff; - Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07; + Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff; + Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07; if (_armed == false) { Motor[chan].SetPoint = 0; @@ -1003,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); + } else { ret = -EINVAL; } @@ -1173,19 +1177,20 @@ mk_start(unsigned motors, char *device_path) { int ret; - // try i2c3 first - g_mk = new MK(3, device_path); + // try i2c3 first + g_mk = new MK(3, device_path); - if (!g_mk) - return -ENOMEM; + if (!g_mk) + return -ENOMEM; - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c3...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); - if (ret > 0) { - return OK; - } - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c3...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + + if (ret > 0) { + return OK; + } + } delete g_mk; g_mk = nullptr; @@ -1194,15 +1199,16 @@ mk_start(unsigned motors, char *device_path) g_mk = new MK(1, device_path); if (!g_mk) - return -ENOMEM; + return -ENOMEM; - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c1...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); - if (ret > 0) { - return OK; - } - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c1...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + + if (ret > 0) { + return OK; + } + } delete g_mk; g_mk = nullptr; @@ -1298,16 +1304,16 @@ mkblctrl_main(int argc, char *argv[]) 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"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } if (!motortest) { if (g_mk == nullptr) { - if (mk_start(motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - } + if (mk_start(motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } /* parameter set ? */ if (newMode) { @@ -1316,18 +1322,19 @@ mkblctrl_main(int argc, char *argv[]) } 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."); + 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); + } else { + g_mk->set_motor_test(motortest); + exit(0); - } - } + } + } } From 30882e103b8551cbdd40db7c4bc4a8a809893d6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 13:22:58 +0100 Subject: [PATCH 08/10] Moved to using references for arrays --- .../fw_att_pos_estimator/estimator.cpp | 23 +++++++++--------- src/modules/fw_att_pos_estimator/estimator.h | 24 +++++++++---------- 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 95408b729b..24e3f4f162 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -53,7 +53,6 @@ float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; -float gpsGndSpd; float gpsVelD; float gpsLat; float gpsLon; @@ -1560,7 +1559,7 @@ void FuseAirspeed() } } -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1573,7 +1572,7 @@ void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) } } -void zeroCols(float covMat[n_states][n_states], uint8_t first, uint8_t last) +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1604,7 +1603,7 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) { long int timeDelta; long int bestTimeDelta = 200; @@ -1630,7 +1629,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec) } } -void quat2Tnb(Mat3f &Tnb, float quat[4]) +void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix float q00 = sq(quat[0]); @@ -1655,7 +1654,7 @@ void quat2Tnb(Mat3f &Tnb, float quat[4]) Tnb.y.z = 2*(q23 + q01); } -void quat2Tbn(Mat3f &Tbn, float quat[4]) +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1680,7 +1679,7 @@ void quat2Tbn(Mat3f &Tbn, float quat[4]) Tbn.z.y = 2*(q23 + q01); } -void eul2quat(float quat[4], float eul[3]) +void eul2quat(float (&quat)[4], const float (&eul)[3]) { float u1 = cos(0.5f*eul[0]); float u2 = cos(0.5f*eul[1]); @@ -1694,28 +1693,28 @@ void eul2quat(float quat[4], float eul[3]) quat[3] = u1*u2*u6-u4*u5*u3; } -void quat2eul(float y[3], float u[4]) +void quat2eul(float (&y)[3], const float (&u)[4]) { y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2])); y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) { velNED[0] = gpsGndSpd*cos(gpsCourse); velNED[1] = gpsGndSpd*sin(gpsCourse); velNED[2] = gpsVelD; } -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); posNED[2] = -(hgt - hgtRef); } -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); @@ -1799,7 +1798,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float initvelNED[3]) +void InitialiseFilter(float (&initvelNED)[3]) { // Do the data structure init for (unsigned i = 0; i < n_states; i++) { diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 2a0e43bb5e..01a1ace996 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,41 +134,39 @@ void FuseMagnetometer(); void FuseAirspeed(); -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last); +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); -void zeroCols(float covMat[n_states][n_states], uint8_t first, uint8_t last); +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); float sq(float valIn); -void quatNorm(float quatOut[4], float quatIn[4]); +void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec); +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec); -void quat2Tnb(Mat3f &Tnb, float quat[4]); - -void quat2Tbn(Mat3f &Tbn, float quat[4]); +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -void eul2quat(float quat[4], float eul[3]); +void eul2quat(float (&quat)[4], const float (&eul)[3]); -void quat2eul(float eul[3],float quat[4]); +void quat2eul(float (&eul)[3], const float (&quat)[4]); -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float initvelNED[3]); +void InitialiseFilter(float (&initvelNED)[3]); uint32_t millis(); From dc2f40b80d6291d4f9b2060bb1d5c86d704a3ca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 14:23:48 +0100 Subject: [PATCH 09/10] Updated uploader from Bootloader repo --- Tools/px_uploader.py | 48 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce5e5e54a..23dc484505 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -50,6 +50,9 @@ # Currently only used for informational purposes. # +# for python2.7 compatibility +from __future__ import print_function + import sys import argparse import binascii @@ -154,6 +157,8 @@ class uploader(object): PROG_MULTI = b'\x27' READ_MULTI = b'\x28' # rev2 only GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -175,6 +180,8 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' def close(self): if self.port is not None: @@ -237,6 +244,22 @@ class uploader(object): self.__getSync() return value + # send the GET_OTP command and wait for an info parameter + def __getOTP(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_OTP + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + + # send the GET_OTP command and wait for an info parameter + def __getSN(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_SN + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -353,6 +376,31 @@ class uploader(object): if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + print("type: " + self.otp_id.decode('utf-8')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print('') print("erase...") self.__erase() From 080b2da214e014933e9062aca9a0be5bd1a2340a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 14:23:48 +0100 Subject: [PATCH 10/10] Updated uploader from Bootloader repo --- Tools/px_uploader.py | 48 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce5e5e54a..23dc484505 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -50,6 +50,9 @@ # Currently only used for informational purposes. # +# for python2.7 compatibility +from __future__ import print_function + import sys import argparse import binascii @@ -154,6 +157,8 @@ class uploader(object): PROG_MULTI = b'\x27' READ_MULTI = b'\x28' # rev2 only GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -175,6 +180,8 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' def close(self): if self.port is not None: @@ -237,6 +244,22 @@ class uploader(object): self.__getSync() return value + # send the GET_OTP command and wait for an info parameter + def __getOTP(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_OTP + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + + # send the GET_OTP command and wait for an info parameter + def __getSN(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_SN + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -353,6 +376,31 @@ class uploader(object): if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + print("type: " + self.otp_id.decode('utf-8')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print('') print("erase...") self.__erase()