diff options
61 files changed, 1950 insertions, 982 deletions
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 000000000..3bf02fbff --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,44 @@ +# Contributing to PX4 Firmware + +We follow the [Github flow](https://guides.github.com/introduction/flow/) development model. + +### Fork the project, then clone your repo + +First [fork and clone](https://help.github.com/articles/fork-a-repo) the project project. + +### Create a feature branch + +*Always* branch off master for new features. + +``` +git checkout -b mydescriptivebranchname +``` + +### Edit and build the code + +The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files. + +### Commit your changes + +Always write descriptive commit messages and add a fixes or relates note to them with an [issue number](https://github.com/px4/Firmware/issues) (Github will link these then conveniently) + +**Example:** + +``` +Change how the attitude controller works + +- Fixes rate feed forward +- Allows a local body rate override + +Fixes issue #123 +``` + +### Test your changes + +Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the logfile from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link. + +### Push your changes + +Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/). + +Make sure to provide some testing feedback and if possible the link to a flight log file. diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index fe85f7d35..c4be16943 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -20,7 +20,7 @@ then param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.28 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index cd4480c3e..e6e2e19dc 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -19,7 +19,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ac2ecc70a..3465b59cf 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -20,7 +20,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 127e42164..0df320f49 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -14,7 +14,7 @@ then param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.5 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f70e0ed77..1c9ded6a8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -132,6 +132,10 @@ then # if param compare SYS_AUTOCONFIG 1 then + # We can't be sure the defaults haven't changed, so + # if someone requests a re-configuration, we do it + # cleanly from scratch (except autostart / autoconfig) + param reset_nostart set DO_AUTOCONFIG yes else set DO_AUTOCONFIG no diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh new file mode 100644 index 000000000..65f0c95da --- /dev/null +++ b/Tools/px_generate_xml.sh @@ -0,0 +1,2 @@ +#!/bin/sh +python px_process_params.py --xml diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 98c491ce6..92e089217 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -559,13 +559,7 @@ BlinkM::led() } /* get number of used satellites in navigation */ - num_of_used_sats = 0; - - for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { - if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { - num_of_used_sats++; - } - } + num_of_used_sats = vehicle_gps_position_raw.satellites_used; if (new_data_vehicle_status || no_data_vehicle_status < 3) { if (num_of_cells == 0) { diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 4b12b75f9..293021f8b 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -99,7 +99,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); __END_DECLS diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index ea91f34ad..ee53fc43d 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -54,13 +54,13 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); __END_DECLS -__EXPORT void led_init() +__EXPORT void led_init(void) { /* Configure LED1-2 GPIOs for output */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index dd505abdb..401b65dd4 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include <drivers/drv_gps.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/satellite_info.h> #include <board_config.h> @@ -79,10 +80,18 @@ #endif static const int ERROR = -1; +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + class GPS : public device::CDev { public: - GPS(const char *uart_path, bool fake_gps); + GPS(const char *uart_path, bool fake_gps, bool enable_sat_info); virtual ~GPS(); virtual int init(); @@ -100,14 +109,17 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - volatile int _task; //< worker task + volatile int _task; ///< worker task bool _healthy; ///< flag to signal if the GPS is ok - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode GPS_Helper *_Helper; ///< instance of GPS parser - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output @@ -154,14 +166,17 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path, bool fake_gps) : +GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), - _report_pub(-1), + _Sat_Info(nullptr), + _report_gps_pos_pub(-1), + _p_report_sat_info(nullptr), + _report_sat_info_pub(-1), _rate(0.0f), _fake_gps(fake_gps) { @@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) : /* we need this potentially before it could be set in task_main */ g_dev = this; - memset(&_report, 0, sizeof(_report)); + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } _debug_enabled = true; } @@ -207,7 +229,7 @@ GPS::init() /* start the GPS driver worker task */ _task = task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -271,33 +293,32 @@ GPS::task_main() if (_fake_gps) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)1200e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph = 0.9f; - _report.epv = 1.8f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } @@ -313,11 +334,11 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); + _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); + _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: @@ -332,20 +353,33 @@ GPS::task_main() // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (helper_ret & 1) { + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + if (_p_report_sat_info && (helper_ret & 2)) { + if (_report_sat_info_pub > 0) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); + } } } - last_rate_count++; + if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + last_rate_count++; + } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { @@ -446,12 +480,13 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); - if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d); - warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv); + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); @@ -469,7 +504,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path, bool fake_gps); +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void stop(); void test(); void reset(); @@ -479,7 +514,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps) +start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; @@ -487,7 +522,7 @@ start(const char *uart_path, bool fake_gps) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path, fake_gps); + g_dev = new GPS(uart_path, fake_gps, enable_sat_info); if (g_dev == nullptr) goto fail; @@ -580,6 +615,7 @@ gps_main(int argc, char *argv[]) /* set to default */ const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; + bool enable_sat_info = false; /* * Start/load the driver. @@ -601,7 +637,13 @@ gps_main(int argc, char *argv[]) fake_gps = true; } - gps::start(device_name, fake_gps); + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps::start(device_name, fake_gps, enable_sat_info); } if (!strcmp(argv[1], "stop")) @@ -626,5 +668,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index d14a95afe..3623397b2 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -62,8 +62,8 @@ protected: uint8_t _rate_count_lat_lon; uint8_t _rate_count_vel; - float _rate_lat_lon; - float _rate_vel; + float _rate_lat_lon = 0.0f; + float _rate_vel = 0.0f; uint64_t _interval_rate_start; }; diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 41716cd97..c4f4f7bec 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - _gps_position->satellites_visible = packet.satellites; + _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 404607571..d0854f5e9 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,14 +34,18 @@ /** * @file ubx.cpp * - * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description * including Prototol Specification. * * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf */ #include <assert.h> @@ -55,24 +59,44 @@ #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/satellite_info.h> #include <drivers/drv_hrt.h> #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK -#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls -#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval + +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) + +#define FNV1_32_INIT ((uint32_t)0x811c9dc5) // init value for FNV1 hash algorithm +#define FNV1_32_PRIME ((uint32_t)0x01000193) // magic prime for FNV1 hash algorithm + -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +/**** Trace macros, disable for production builds */ +#define UBX_TRACE_PARSER(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* decoding progress in parse_char() */ +#define UBX_TRACE_RXMSG(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ +#define UBX_TRACE_SVINFO(s, ...) {/*printf(s, ## __VA_ARGS__);*/} /* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */ + +/**** Warning macros, disable to save memory */ +#define UBX_WARN(s, ...) {warnx(s, ## __VA_ARGS__);} + + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : _fd(fd), _gps_position(gps_position), + _satellite_info(satellite_info), _configured(false), - _waiting_for_ack(false), + _ack_state(UBX_ACK_IDLE), _got_posllh(false), _got_velned(false), - _got_timeutc(false), - _disable_cmd_last(0) + _disable_cmd_last(0), + _ack_waiting_msg(0), + _ubx_version(0), + _use_nav_pvt(false) { decode_init(); } @@ -86,182 +110,167 @@ UBX::configure(unsigned &baudrate) { _configured = false; /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200}; - int baud_i; + unsigned baud_i; - for (baud_i = 0; baud_i < 5; baud_i++) { - baudrate = baudrates_to_try[baud_i]; + for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) { + baudrate = baudrates[baud_i]; set_baudrate(_fd, baudrate); + /* flush input and wait for at least 20 ms silence */ + decode_init(); + receive(20); + decode_init(); + /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_PRT; - - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + * and leave the baudrate as it is, we just want an ACK-ACK for this */ + memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = baudrate; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; + + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); + + if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) { /* try next baudrate */ continue; } /* Send a CFG-PRT message again, this time change the baudrate */ + memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt)); + _buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID; + _buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE; + _buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE; + _buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK; + _buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK; - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt)); /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - wait_for_ack(UBX_CONFIG_TIMEOUT); + wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false); - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); - baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE); + baudrate = UBX_TX_CFG_PRT_BAUDRATE; } /* at this point we have correct baudrate on both ends */ break; } - if (baud_i >= 5) { - return 1; + if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { + return 1; // connection and/or baudrate detection failed } - /* send a CFG-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + /* Send a CFG-RATE message to define update rate */ + memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate)); + _buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL; + _buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE; + _buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF; - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_RATE; + send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate)); - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - - send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: RATE"); + if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_NAV5; + memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5)); + _buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK; + _buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL; + _buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE; - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5)); - send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("CFG FAIL: NAV5"); + if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV POSLLH"); - return 1; + /* try to set rate for NAV-PVT */ + /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ + configure_message_rate(UBX_MSG_NAV_PVT, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + _use_nav_pvt = false; + } else { + _use_nav_pvt = true; } + UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + if (!_use_nav_pvt) { + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV TIMEUTC"); - return 1; - } + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SOL"); - return 1; + configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV VELNED"); + configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); + configure_message_rate(UBX_MSG_MON_HW, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); - return 1; - } + /* request module version information by sending an empty MON-VER message */ + send_message(UBX_MSG_MON_VER, nullptr, 0); _configured = true; return 0; } -int -UBX::wait_for_ack(unsigned timeout) +int // -1 = NAK, error or timeout, 0 = ACK +UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) { - _waiting_for_ack = true; - uint64_t time_started = hrt_absolute_time(); + int ret = -1; - while (hrt_absolute_time() < time_started + timeout * 1000) { - if (receive(timeout) > 0) { - if (!_waiting_for_ack) { - return 1; - } + _ack_state = UBX_ACK_WAITING; + _ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check + + hrt_abstime time_started = hrt_absolute_time(); + while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) { + receive(timeout); + } + + if (_ack_state == UBX_ACK_GOT_ACK) { + ret = 0; // ACK received ok + } else if (report) { + if (_ack_state == UBX_ACK_GOT_NAK) { + UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); } else { - return -1; // timeout or error receiving, or NAK + UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } } - return -1; // timeout + _ack_state = UBX_ACK_IDLE; + return ret; } -int -UBX::receive(unsigned timeout) +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX::receive(const unsigned timeout) { /* poll descriptor */ pollfd fds[1]; @@ -275,17 +284,17 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool handled = false; + int handled = 0; while (true) { - bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; + bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ - warnx("poll error"); + UBX_WARN("ubx poll() err"); return -1; } else if (ret == 0) { @@ -293,8 +302,7 @@ UBX::receive(unsigned timeout) if (ready_to_return) { _got_posllh = false; _got_velned = false; - _got_timeutc = false; - return 1; + return handled; } else { return -1; @@ -314,440 +322,675 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i]) > 0) { - if (handle_message() > 0) - handled = true; - } + handled |= parse_char(buf[i]); } } } /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { - warnx("timeout - no useful messages"); return -1; } } } -int -UBX::parse_char(uint8_t b) +int // 0 = decoding, 1 = message handled, 2 = sat info message handled +UBX::parse_char(const uint8_t b) { + int ret = 0; + switch (_decode_state) { - /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } + /* Expecting Sync1 */ + case UBX_DECODE_SYNC1: + if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2 + UBX_TRACE_PARSER("\nA"); + _decode_state = UBX_DECODE_SYNC2; + } break; - /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; + /* Expecting Sync2 */ + case UBX_DECODE_SYNC2: + if (b == UBX_SYNC2) { // Sync2 found --> expecting Class + UBX_TRACE_PARSER("B"); + _decode_state = UBX_DECODE_CLASS; - } else { - /* Second start symbol was wrong, reset state machine */ + } else { // Sync1 not followed by Sync2: reset parser decode_init(); - /* don't return error, it can be just false sync1 */ } + break; + /* Expecting Class */ + case UBX_DECODE_CLASS: + UBX_TRACE_PARSER("C"); + add_byte_to_checksum(b); // checksum is calculated for everything except Sync and Checksum bytes + _rx_msg = b; + _decode_state = UBX_DECODE_ID; break; - /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ + /* Expecting ID */ + case UBX_DECODE_ID: + UBX_TRACE_PARSER("D"); add_byte_to_checksum(b); - _message_class = b; - _decode_state = UBX_DECODE_GOT_CLASS; + _rx_msg |= b << 8; + _decode_state = UBX_DECODE_LENGTH1; break; - case UBX_DECODE_GOT_CLASS: + /* Expecting first length byte */ + case UBX_DECODE_LENGTH1: + UBX_TRACE_PARSER("E"); add_byte_to_checksum(b); - _message_id = b; - _decode_state = UBX_DECODE_GOT_MESSAGEID; + _rx_payload_length = b; + _decode_state = UBX_DECODE_LENGTH2; break; - case UBX_DECODE_GOT_MESSAGEID: + /* Expecting second length byte */ + case UBX_DECODE_LENGTH2: + UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; + _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception + // payload will not be handled, discard message + decode_init(); + } else { + _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; + } break; - case UBX_DECODE_GOT_LENGTH1: + /* Expecting payload */ + case UBX_DECODE_PAYLOAD: + UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; + switch (_rx_msg) { + case UBX_MSG_NAV_SVINFO: + ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte + break; + case UBX_MSG_MON_VER: + ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte + break; + default: + ret = payload_rx_add(b); // add a payload byte + break; + } + if (ret < 0) { + // payload not handled, discard message + decode_init(); + } else if (ret > 0) { + // payload complete, expecting checksum + _decode_state = UBX_DECODE_CHKSUM1; + } else { + // expecting more payload, stay in state UBX_DECODE_PAYLOAD + } + ret = 0; break; - case UBX_DECODE_GOT_LENGTH2: + /* Expecting first checksum byte */ + case UBX_DECODE_CHKSUM1: + if (_rx_ck_a != b) { + UBX_WARN("ubx checksum err"); + decode_init(); + } else { + _decode_state = UBX_DECODE_CHKSUM2; + } + break; - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); + /* Expecting second checksum byte */ + case UBX_DECODE_CHKSUM2: + if (_rx_ck_b != b) { + UBX_WARN("ubx checksum err"); + } else { + ret = payload_rx_done(); // finish payload processing + } + decode_init(); + break; - _rx_buffer[_rx_count] = b; + default: + break; + } - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { - decode_init(); - return 1; // message received successfully + return ret; +} - } else { - warnx("checksum wrong"); - decode_init(); - return -1; - } +/** + * Start payload rx + */ +int // -1 = abort, 0 = continue +UBX::payload_rx_init() +{ + int ret = 0; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; + _rx_state = UBX_RXMSG_HANDLE; // handle by default + + switch (_rx_msg) { + case UBX_MSG_NAV_PVT: + if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (!_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + break; - } else { - warnx("buffer full"); - decode_init(); - return -1; - } + case UBX_MSG_NAV_POSLLH: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + break; + case UBX_MSG_NAV_SOL: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + break; + + case UBX_MSG_NAV_TIMEUTC: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + break; + + case UBX_MSG_NAV_SVINFO: + if (_satellite_info == nullptr) + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + break; + + case UBX_MSG_NAV_VELNED: + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + else if (_use_nav_pvt) + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + break; + + case UBX_MSG_MON_VER: + break; // unconditionally handle this message + + case UBX_MSG_MON_HW: + if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (!_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + break; + + case UBX_MSG_ACK_ACK: + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + break; + + case UBX_MSG_ACK_NAK: + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + _rx_state = UBX_RXMSG_ERROR_LENGTH; + else if (_configured) + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured break; default: + _rx_state = UBX_RXMSG_DISABLE; // disable all other messages break; } - return 0; // message decoding in progress + switch (_rx_state) { + case UBX_RXMSG_HANDLE: // handle message + case UBX_RXMSG_IGNORE: // ignore message but don't report error + ret = 0; + break; + + case UBX_RXMSG_DISABLE: // disable unexpected messages + UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + + { + hrt_abstime t = hrt_absolute_time(); + + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { + /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; + UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg)); + configure_message_rate(_rx_msg, 0); + } + } + + ret = -1; // return error, abort handling this message + break; + + case UBX_RXMSG_ERROR_LENGTH: // error: invalid length + UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length); + ret = -1; // return error, abort handling this message + break; + + default: // invalid message state + UBX_WARN("ubx internal err1"); + ret = -1; // return error, abort handling this message + break; + } + + return ret; } +/** + * Add payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add(const uint8_t b) +{ + int ret = 0; + _buf.raw[_rx_payload_index] = b; -int -UBX::handle_message() + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } + + return ret; +} + +/** + * Add NAV-SVINFO payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_nav_svinfo(const uint8_t b) { int ret = 0; - if (_configured) { - /* handle only info messages when configured */ - switch (_message_class) { - case UBX_CLASS_NAV: - switch (_message_id) { - case UBX_MESSAGE_NAV_POSLLH: { - // printf("GOT NAV_POSLLH\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; - _gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m - _gps_position->timestamp_position = hrt_absolute_time(); - - _rate_count_lat_lon++; - - _got_posllh = true; - ret = 1; - break; - } + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_rx_payload_index] = b; + } else { + if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { + // Part 1 complete: decode Part 1 buffer + _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + } + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + // Still room in _satellite_info: fill Part 2 buffer + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { + // Part 2 complete: decode Part 2 buffer + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); + _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); + _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); + _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); + _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); + UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); + } + } + } + + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } - case UBX_MESSAGE_NAV_SOL: { - // printf("GOT NAV_SOL\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + return ret; +} - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s - _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m - _gps_position->timestamp_variance = hrt_absolute_time(); +/** + * Add MON-VER payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_mon_ver(const uint8_t b) +{ + int ret = 0; - ret = 1; - break; - } + if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Fill Part 1 buffer + _buf.raw[_rx_payload_index] = b; + } else { + if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { + // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings + _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT); + _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version); + UBX_WARN("VER hash 0x%08x", _ubx_version); + UBX_WARN("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); + UBX_WARN("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); + } + // fill Part 2 buffer + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { + // Part 2 complete: decode Part 2 buffer + UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); + } + } - case UBX_MESSAGE_NAV_TIMEUTC: { - // printf("GOT NAV_TIMEUTC\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } - /* convert to unix timestamp */ - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - time_t epoch = mktime(&timeinfo); + return ret; +} + +/** + * Finish payload rx + */ +int // 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX::payload_rx_done(void) +{ + int ret = 0; + + // return if no message handled + if (_rx_state != UBX_RXMSG_HANDLE) { + return ret; + } + + // handle message + switch (_rx_msg) { + + case UBX_MSG_NAV_PVT: + UBX_TRACE_RXMSG("Rx NAV-PVT\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; + + _gps_position->lat = _buf.payload_rx_nav_pvt.lat; + _gps_position->lon = _buf.payload_rx_nav_pvt.lon; + _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; + + _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; + _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; + + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; + _gps_position->vel_ned_valid = true; + + _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; + timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; + timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; + timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; + time_t epoch = mktime(&timeinfo); #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME, &ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + clock_settime(CLOCK_REALTIME, &ts); #endif - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + } - _got_timeutc = true; - ret = 1; - break; - } + _gps_position->timestamp_time = hrt_absolute_time(); + _gps_position->timestamp_velocity = hrt_absolute_time(); + _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->timestamp_position = hrt_absolute_time(); - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - uint8_t satellites_used = 0; - int i; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - - /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; - break; - } + _rate_count_vel++; + _rate_count_lat_lon++; - case UBX_MESSAGE_NAV_VELNED: { - // printf("GOT NAV_VELNED\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + _got_posllh = true; + _got_velned = true; - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); + ret = 1; + break; - _rate_count_vel++; + case UBX_MSG_NAV_POSLLH: + UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); - _got_velned = true; - ret = 1; - break; - } + _gps_position->lat = _buf.payload_rx_nav_posllh.lat; + _gps_position->lon = _buf.payload_rx_nav_posllh.lon; + _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; + _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m + _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m - default: - break; - } + _gps_position->timestamp_position = hrt_absolute_time(); - break; + _rate_count_lat_lon++; + _got_posllh = true; - case UBX_CLASS_ACK: { - /* ignore ACK when already configured */ - ret = 1; - break; - } + ret = 1; + break; - case UBX_CLASS_MON: { - switch (_message_id) { - case UBX_MESSAGE_MON_HW: { + case UBX_MSG_NAV_SOL: + UBX_TRACE_RXMSG("Rx NAV-SOL\n"); - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m + _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; + _gps_position->timestamp_variance = hrt_absolute_time(); - ret = 1; - break; - } + ret = 1; + break; - default: - break; - } + case UBX_MSG_NAV_TIMEUTC: + UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); + + { + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; + timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; + timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day; + timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; + timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; + timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; + time_t epoch = mktime(&timeinfo); + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + clock_settime(CLOCK_REALTIME, &ts); +#endif + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); } - default: + _gps_position->timestamp_time = hrt_absolute_time(); + + ret = 1; + break; + + case UBX_MSG_NAV_SVINFO: + UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); + + // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp + _satellite_info->timestamp = hrt_absolute_time(); + + ret = 2; + break; + + case UBX_MSG_NAV_VELNED: + UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); + + _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f; + _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + + _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; + _got_velned = true; + + ret = 1; + break; + + case UBX_MSG_MON_VER: + UBX_TRACE_RXMSG("Rx MON-VER\n"); + + ret = 1; + break; + + case UBX_MSG_MON_HW: + UBX_TRACE_RXMSG("Rx MON-HW\n"); + + switch (_rx_payload_length) { + + case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */ + _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS; + _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd; + + ret = 1; break; - } - if (ret == 0) { - /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */ + _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS; + _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd; - hrt_abstime t = hrt_absolute_time(); + ret = 1; + break; - if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { - /* don't attempt for every message to disable, some might not be disabled */ - _disable_cmd_last = t; - warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); - } + default: // unexpected payload size: + ret = 0; // don't handle message + break; } + break; - } else { - /* handle only ACK while configuring */ - if (_message_class == UBX_CLASS_ACK) { - switch (_message_id) { - case UBX_MESSAGE_ACK_ACK: { - // printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { - _waiting_for_ack = false; - ret = 1; - } - } - - break; - } + case UBX_MSG_ACK_ACK: + UBX_TRACE_RXMSG("Rx ACK-ACK\n"); - case UBX_MESSAGE_ACK_NAK: { - // printf("GOT ACK_NAK\n"); - warnx("ubx: not acknowledged"); - /* configuration obviously not successful */ - _waiting_for_ack = false; - ret = -1; - break; - } + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_ACK; + } - default: - break; - } + ret = 1; + break; + + case UBX_MSG_ACK_NAK: + UBX_TRACE_RXMSG("Rx ACK-NAK\n"); + + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_NAK; } + + ret = 1; + break; + + default: + break; } - decode_init(); return ret; } void UBX::decode_init(void) { + _decode_state = UBX_DECODE_SYNC1; _rx_ck_a = 0; _rx_ck_b = 0; - _rx_count = 0; - _decode_state = UBX_DECODE_UNINIT; - _payload_size = 0; - /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ + _rx_payload_length = 0; + _rx_payload_index = 0; } void -UBX::add_byte_to_checksum(uint8_t b) +UBX::add_byte_to_checksum(const uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void -UBX::add_checksum_to_message(uint8_t *message, const unsigned length) +UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) { - uint8_t ck_a = 0; - uint8_t ck_b = 0; - unsigned i; - - for (i = 0; i < length - 2; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; + for (uint16_t i = 0; i < length; i++) { + checksum->ck_a = checksum->ck_a + buffer[i]; + checksum->ck_b = checksum->ck_b + checksum->ck_a; } - - /* the checksum is written to the last to bytes of a message */ - message[length - 2] = ck_a; - message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::configure_message_rate(const uint16_t msg, const uint8_t rate) { - for (unsigned i = 0; i < length; i++) { - ck_a = ck_a + message[i]; - ck_b = ck_b + ck_a; - } + ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation) + + cfg_msg.msg = msg; + cfg_msg.rate = rate; + + send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg)); } void -UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) +UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length) { - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + ubx_header_t header = {UBX_SYNC1, UBX_SYNC2}; + ubx_checksum_t checksum = {0, 0}; + + // Populate header + header.msg = msg; + header.length = length; + + // Calculate checksum + calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + if (payload != nullptr) + calc_checksum(payload, length, &checksum); + + // Send message + write(_fd, (const void *)&header, sizeof(header)); + if (payload != nullptr) + write(_fd, (const void *)payload, length); + write(_fd, (const void *)&checksum, sizeof(checksum)); } -void -UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +uint32_t +UBX::fnv1_32_str(uint8_t *str, uint32_t hval) { - ssize_t ret = 0; - - /* calculate the checksum now */ - add_checksum_to_message(packet, length); - - const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + uint8_t *s = str; + + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { + + /* multiply by the 32 bit FNV magic prime mod 2^32 */ +#if defined(NO_FNV_GCC_OPTIMIZATION) + hval *= FNV1_32_PRIME; +#else + hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); +#endif - /* start with the two sync bytes */ - ret += write(fd, sync_bytes, sizeof(sync_bytes)); - ret += write(fd, packet, length); + /* xor the bottom with the current octet */ + hval ^= (uint32_t)*s++; + } - if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: configuration write fail"); + /* return our new hash value */ + return hval; } -void -UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) -{ - struct ubx_header header; - uint8_t ck_a = 0, ck_b = 0; - header.sync1 = UBX_SYNC1; - header.sync2 = UBX_SYNC2; - header.msg_class = msg_class; - header.msg_id = msg_id; - header.length = size; - - add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); - add_checksum((uint8_t *)msg, size, ck_a, ck_b); - - /* configure ACK check */ - _message_class_needed = msg_class; - _message_id_needed = msg_id; - - write(_fd, (const char *)&header, sizeof(header)); - write(_fd, (const char *)msg, size); - write(_fd, (const char *)&ck_a, 1); - write(_fd, (const char *)&ck_b, 1); -} diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 43d688893..219a5762a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,13 +34,16 @@ /** * @file ubx.h * - * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description * including Prototol Specification. * * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * */ #ifndef UBX_H_ @@ -51,319 +54,433 @@ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -/* ClassIDs (the ones that are used) */ -#define UBX_CLASS_NAV 0x01 -//#define UBX_CLASS_RXM 0x02 -#define UBX_CLASS_ACK 0x05 -#define UBX_CLASS_CFG 0x06 -#define UBX_CLASS_MON 0x0A - -/* MessageIDs (the ones that are used) */ -#define UBX_MESSAGE_NAV_POSLLH 0x02 -//#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_VELNED 0x12 -//#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_SVINFO 0x30 -#define UBX_MESSAGE_ACK_NAK 0x00 -#define UBX_MESSAGE_ACK_ACK 0x01 -#define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_MSG 0x01 -#define UBX_MESSAGE_CFG_RATE 0x08 -#define UBX_MESSAGE_CFG_NAV5 0x24 - -#define UBX_MESSAGE_MON_HW 0x09 - -#define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ - -#define UBX_CFG_RATE_LENGTH 6 -#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */ -#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ -#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ - - -#define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ -#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ - -#define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ -#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 - -#define UBX_MAX_PAYLOAD_LENGTH 500 - -// ************ -/** the structures of the binary packets */ +/* Message Classes */ +#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_CLASS_MON 0x0A + +/* Message IDs */ +#define UBX_ID_NAV_POSLLH 0x02 +#define UBX_ID_NAV_SOL 0x06 +#define UBX_ID_NAV_PVT 0x07 +#define UBX_ID_NAV_VELNED 0x12 +#define UBX_ID_NAV_TIMEUTC 0x21 +#define UBX_ID_NAV_SVINFO 0x30 +#define UBX_ID_ACK_NAK 0x00 +#define UBX_ID_ACK_ACK 0x01 +#define UBX_ID_CFG_PRT 0x00 +#define UBX_ID_CFG_MSG 0x01 +#define UBX_ID_CFG_RATE 0x08 +#define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_MON_VER 0x04 +#define UBX_ID_MON_HW 0x09 + +/* Message Classes & IDs */ +#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8) +#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8) +#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8) +#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8) +#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8) +#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8) +#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8) +#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8) +#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8) +#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) +#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) +#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) +#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) + +/* RX NAV-PVT message content details */ +/* Bitfield "valid" masks */ +#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */ +#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */ +#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */ + +/* Bitfield "flags" masks */ +#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */ +#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */ +#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */ +#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */ + +/* RX NAV-TIMEUTC message content details */ +/* Bitfield "valid" masks */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW 0x01 /**< validTOW (1 = Valid Time of Week) */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN 0x02 /**< validWKN (1 = Valid Week Number) */ +#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC 0x04 /**< validUTC (1 = Valid UTC Time) */ +#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */ + +/* TX CFG-PRT message contents */ +#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ +#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ +#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */ + +/* TX CFG-RATE message contents */ +#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */ +#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */ +#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */ + +/* TX CFG-NAV5 message contents */ +#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */ +#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ +#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ + +/* TX CFG-MSG message contents */ +#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_TX_CFG_MSG_RATE1_05HZ 10 + + +/*** u-blox protocol binary message and payload definitions ***/ #pragma pack(push, 1) -struct ubx_header { - uint8_t sync1; - uint8_t sync2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -}; - +/* General: Header */ typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t lon; /**< Longitude * 1e-7, deg */ - int32_t lat; /**< Latitude * 1e-7, deg */ - int32_t height; /**< Height above Ellipsoid, mm */ - int32_t height_msl; /**< Height above mean sea level, mm */ - uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ - uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_posllh_packet_t; + uint8_t sync1; + uint8_t sync2; + uint16_t msg; + uint16_t length; +} ubx_header_t; +/* General: Checksum */ typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ - int16_t week; /**< GPS week (GPS time) */ - uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ - uint8_t flags; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - int32_t ecefVX; - int32_t ecefVY; - int32_t ecefVZ; - uint32_t sAcc; - uint16_t pDOP; - uint8_t reserved1; - uint8_t numSV; - uint32_t reserved2; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_sol_packet_t; + uint8_t ck_a; + uint8_t ck_b; +} ubx_checksum_t ; +/* Rx NAV-POSLLH */ typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ - int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ - uint16_t year; /**< Year, range 1999..2099 (UTC) */ - uint8_t month; /**< Month, range 1..12 (UTC) */ - uint8_t day; /**< Day of Month, range 1..31 (UTC) */ - uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ - uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ - uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ - uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_timeutc_packet_t; - -//typedef struct { -// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ -// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ -// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ -// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ -// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ -// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ -// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ -// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ -// uint8_t ck_a; -// uint8_t ck_b; -//} gps_bin_nav_dop_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t lon; /**< Longitude [1e-7 deg] */ + int32_t lat; /**< Latitude [1e-7 deg] */ + int32_t height; /**< Height above ellipsoid [mm] */ + int32_t hMSL; /**< Height above mean sea level [mm] */ + uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ + uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ +} ubx_payload_rx_nav_posllh_t; + +/* Rx NAV-SOL */ typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ - uint8_t globalFlags; - uint16_t reserved2; - -} gps_bin_nav_svinfo_part1_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */ + int16_t week; /**< GPS week */ + uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint32_t reserved2; +} ubx_payload_rx_nav_sol_t; + +/* Rx NAV-PVT (ubx8) */ typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ - uint8_t flags; - uint8_t quality; - uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ - int8_t elev; /**< Elevation in integer degrees */ - int16_t azim; /**< Azimuth in integer degrees */ - int32_t prRes; /**< Pseudo range residual in centimetres */ - -} gps_bin_nav_svinfo_part2_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint16_t year; /**< Year (UTC)*/ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ + uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */ + uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ + int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */ + uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */ + uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */ + uint8_t reserved1; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ + int32_t lon; /**< Longitude [1e-7 deg] */ + int32_t lat; /**< Latitude [1e-7 deg] */ + int32_t height; /**< Height above ellipsoid [mm] */ + int32_t hMSL; /**< Height above mean sea level [mm] */ + uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */ + uint32_t vAcc; /**< Vertical accuracy estimate [mm] */ + int32_t velN; /**< NED north velocity [mm/s]*/ + int32_t velE; /**< NED east velocity [mm/s]*/ + int32_t velD; /**< NED down velocity [mm/s]*/ + int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */ + int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */ + uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */ + uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */ + uint16_t pDOP; /**< Position DOP [0.01] */ + uint16_t reserved2; + uint32_t reserved3; + int32_t headVeh; /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; /**< (ubx8+ only) */ +} ubx_payload_rx_nav_pvt_t; +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7 (sizeof(ubx_payload_rx_nav_pvt_t) - 8) +#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8 (sizeof(ubx_payload_rx_nav_pvt_t)) + +/* Rx NAV-TIMEUTC */ typedef struct { - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_svinfo_part3_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */ + int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */ + uint8_t valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */ +} ubx_payload_rx_nav_timeutc_t; + +/* Rx NAV-SVINFO Part 1 */ typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t velN; //NED north velocity, cm/s - int32_t velE; //NED east velocity, cm/s - int32_t velD; //NED down velocity, cm/s - uint32_t speed; //Speed (3-D), cm/s - uint32_t gSpeed; //Ground Speed (2-D), cm/s - int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 - uint32_t sAcc; //Speed Accuracy Estimate, cm/s - uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_velned_packet_t; - -struct gps_bin_mon_hw_packet { - uint32_t pinSel; - uint32_t pinBank; - uint32_t pinDir; - uint32_t pinVal; - uint16_t noisePerMS; - uint16_t agcCnt; - uint8_t aStatus; - uint8_t aPower; - uint8_t flags; - uint8_t __reserved1; - uint32_t usedMask; - uint8_t VP[25]; - uint8_t jamInd; - uint16_t __reserved3; - uint32_t pinIrq; - uint32_t pulLH; - uint32_t pullL; -}; - - -//typedef struct { -// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ -// int16_t week; /**< Measurement GPS week number */ -// uint8_t numVis; /**< Number of visible satellites */ -// -// //... rest of package is not used in this implementation -// -//} gps_bin_rxm_svsi_packet_t; + uint32_t iTOW; /**< GPS Time of Week [ms] */ + uint8_t numCh; /**< Number of channels */ + uint8_t globalFlags; + uint16_t reserved2; +} ubx_payload_rx_nav_svinfo_part1_t; +/* Rx NAV-SVINFO Part 2 (repeated) */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_ack_packet_t; - + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ + uint8_t flags; + uint8_t quality; + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */ + int8_t elev; /**< Elevation [deg] */ + int16_t azim; /**< Azimuth [deg] */ + int32_t prRes; /**< Pseudo range residual [cm] */ +} ubx_payload_rx_nav_svinfo_part2_t; + +/* Rx NAV-VELNED */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_nak_packet_t; - + uint32_t iTOW; /**< GPS Time of Week [ms] */ + int32_t velN; /**< North velocity component [cm/s]*/ + int32_t velE; /**< East velocity component [cm/s]*/ + int32_t velD; /**< Down velocity component [cm/s]*/ + uint32_t speed; /**< Speed (3-D) [cm/s] */ + uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */ + int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */ + uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */ + uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */ +} ubx_payload_rx_nav_velned_t; + +/* Rx MON-HW (ubx6) */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t portID; - uint8_t res0; - uint16_t res1; - uint32_t mode; - uint32_t baudRate; - uint16_t inProtoMask; - uint16_t outProtoMask; - uint16_t flags; - uint16_t pad; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_prt_packet_t; - + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[25]; + uint8_t jamInd; + uint16_t reserved3; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; +} ubx_payload_rx_mon_hw_ubx6_t; + +/* Rx MON-HW (ubx7+) */ +typedef struct { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[17]; + uint8_t jamInd; + uint16_t reserved3; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; +} ubx_payload_rx_mon_hw_ubx7_t; + +/* Rx MON-VER Part 1 */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t measRate; - uint16_t navRate; - uint16_t timeRef; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_rate_packet_t; + uint8_t swVersion[30]; + uint8_t hwVersion[10]; +} ubx_payload_rx_mon_ver_part1_t; +/* Rx MON-VER Part 2 (repeated) */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint16_t mask; - uint8_t dynModel; - uint8_t fixMode; - int32_t fixedAlt; - uint32_t fixedAltVar; - int8_t minElev; - uint8_t drLimit; - uint16_t pDop; - uint16_t tDop; - uint16_t pAcc; - uint16_t tAcc; - uint8_t staticHoldThresh; - uint8_t dgpsTimeOut; - uint32_t reserved2; - uint32_t reserved3; - uint32_t reserved4; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_nav5_packet_t; + uint8_t extension[30]; +} ubx_payload_rx_mon_ver_part2_t; + +/* Rx ACK-ACK */ +typedef union { + uint16_t msg; + struct { + uint8_t clsID; + uint8_t msgID; + }; +} ubx_payload_rx_ack_ack_t; + +/* Rx ACK-NAK */ +typedef union { + uint16_t msg; + struct { + uint8_t clsID; + uint8_t msgID; + }; +} ubx_payload_rx_ack_nak_t; + +/* Tx CFG-PRT */ +typedef struct { + uint8_t portID; + uint8_t reserved0; + uint16_t txReady; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t reserved5; +} ubx_payload_tx_cfg_prt_t; + +/* Tx CFG-RATE */ +typedef struct { + uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */ + uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */ + uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */ +} ubx_payload_tx_cfg_rate_t; +/* Tx CFG-NAV5 */ +typedef struct { + uint16_t mask; + uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ + uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */ + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */ + uint8_t cnoThresh; /**< (ubx7+ only, else 0) */ + uint16_t reserved; + uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */ + uint8_t utcStandard; /**< (ubx8+ only, else 0) */ + uint8_t reserved3; + uint32_t reserved4; +} ubx_payload_tx_cfg_nav5_t; + +/* Tx CFG-MSG */ typedef struct { - uint8_t clsID; - uint8_t msgID; - uint16_t length; - uint8_t msgClass_payload; - uint8_t msgID_payload; + union { + uint16_t msg; + struct { + uint8_t msgClass; + uint8_t msgID; + }; + }; uint8_t rate; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_packet_t; +} ubx_payload_tx_cfg_msg_t; + +/* General message and payload buffer union */ +typedef union { + ubx_payload_rx_nav_pvt_t payload_rx_nav_pvt; + ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh; + ubx_payload_rx_nav_sol_t payload_rx_nav_sol; + ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc; + ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1; + ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2; + ubx_payload_rx_nav_velned_t payload_rx_nav_velned; + ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6; + ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7; + ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1; + ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2; + ubx_payload_rx_ack_ack_t payload_rx_ack_ack; + ubx_payload_rx_ack_nak_t payload_rx_ack_nak; + ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; + ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; + ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; + uint8_t raw[]; +} ubx_buf_t; -struct ubx_cfg_msg_rate { - uint8_t msg_class; - uint8_t msg_id; - uint8_t rate; -}; - - -// END the structures of the binary packets -// ************ +#pragma pack(pop) +/*** END OF u-blox protocol binary message and payload definitions ***/ +/* Decoder state */ typedef enum { - UBX_DECODE_UNINIT = 0, - UBX_DECODE_GOT_SYNC1, - UBX_DECODE_GOT_SYNC2, - UBX_DECODE_GOT_CLASS, - UBX_DECODE_GOT_MESSAGEID, - UBX_DECODE_GOT_LENGTH1, - UBX_DECODE_GOT_LENGTH2 + UBX_DECODE_SYNC1 = 0, + UBX_DECODE_SYNC2, + UBX_DECODE_CLASS, + UBX_DECODE_ID, + UBX_DECODE_LENGTH1, + UBX_DECODE_LENGTH2, + UBX_DECODE_PAYLOAD, + UBX_DECODE_CHKSUM1, + UBX_DECODE_CHKSUM2 } ubx_decode_state_t; -//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; -#pragma pack(pop) +/* Rx message state */ +typedef enum { + UBX_RXMSG_IGNORE = 0, + UBX_RXMSG_HANDLE, + UBX_RXMSG_DISABLE, + UBX_RXMSG_ERROR_LENGTH +} ubx_rxmsg_state_t; + +/* ACK state */ +typedef enum { + UBX_ACK_IDLE = 0, + UBX_ACK_WAITING, + UBX_ACK_GOT_ACK, + UBX_ACK_GOT_NAK +} ubx_ack_state_t; -#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); ~UBX(); - int receive(unsigned timeout); + int receive(const unsigned timeout); int configure(unsigned &baudrate); private: /** - * Parse the binary MTK packet + * Parse the binary UBX packet + */ + int parse_char(const uint8_t b); + + /** + * Start payload rx */ - int parse_char(uint8_t b); + int payload_rx_init(void); /** - * Handle the package once it has arrived + * Add payload rx byte */ - int handle_message(void); + int payload_rx_add(const uint8_t b); + int payload_rx_add_nav_svinfo(const uint8_t b); + int payload_rx_add_mon_ver(const uint8_t b); + + /** + * Finish payload rx + */ + int payload_rx_done(void); /** * Reset the parse state machine for a fresh start @@ -373,44 +490,53 @@ private: /** * While parsing add every byte (except the sync bytes) to the checksum */ - void add_byte_to_checksum(uint8_t); + void add_byte_to_checksum(const uint8_t); /** - * Add the two checksum bytes to an outgoing message + * Send a message */ - void add_checksum_to_message(uint8_t *message, const unsigned length); + void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length); /** - * Helper to send a config packet + * Configure message rate */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + void configure_message_rate(const uint16_t msg, const uint8_t rate); - void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); - - void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); + /** + * Calculate & add checksum for given buffer + */ + void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum); - void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + /** + * Wait for message acknowledge + */ + int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); - int wait_for_ack(unsigned timeout); + /** + * Calculate FNV1 hash + */ + uint32_t fnv1_32_str(uint8_t *str, uint32_t hval); int _fd; struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; bool _configured; - bool _waiting_for_ack; + ubx_ack_state_t _ack_state; bool _got_posllh; bool _got_velned; - bool _got_timeutc; - uint8_t _message_class_needed; - uint8_t _message_id_needed; ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; + uint16_t _rx_msg; + ubx_rxmsg_state_t _rx_state; + uint16_t _rx_payload_length; + uint16_t _rx_payload_index; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - uint8_t _message_class; - uint8_t _message_id; - unsigned _payload_size; hrt_abstime _disable_cmd_last; + uint16_t _ack_waiting_msg; + ubx_buf_t _buf; + uint32_t _ubx_version; + bool _use_nav_pvt; }; #endif /* UBX_H_ */ diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1e779e8dc..086132573 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - msg.gps_num_sat = gps.satellites_visible; + msg.gps_num_sat = gps.satellites_used; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 992ab9623..24da4c68b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include <systemlib/systemlib.h> #include <systemlib/scheduling_priorities.h> #include <systemlib/param/param.h> +#include <systemlib/circuit_breaker.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -1012,6 +1013,19 @@ PX4IO::task_main() } } + int32_t safety_param_val; + param_t safety_param = param_find("RC_FAILS_THR"); + + if (safety_param != PARAM_INVALID) { + + param_get(safety_param, &safety_param_val); + + if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) { + /* disable IO safety if circuit breaker asked for it */ + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val); + } + } + } } diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 3730b1920..6386e37a0 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { - ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr); } else { ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 24da452b1..be465ab76 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -131,6 +131,7 @@ #include <fcntl.h> #include <sys/prctl.h> #include <math.h> +#include <float.h> #include <mathlib/mathlib.h> #include <string.h> #include <drivers/drv_hrt.h> @@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) { + if (fabsf(det) < FLT_EPSILON) { return ERROR; // Singular matrix } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 9d79124e7..43f341ae7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -40,6 +40,7 @@ */ #include <math.h> +#include <float.h> #include "calibration_routines.h" @@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; + aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; + aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; + aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; //Compute next iteration nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb42889ea..efa26eb97 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include <fcntl.h> #include <errno.h> #include <systemlib/err.h> +#include <systemlib/circuit_breaker.h> #include <debug.h> #include <sys/prctl.h> #include <sys/stat.h> @@ -76,6 +77,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> +#include <uORB/topics/system_power.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/telemetry_status.h> @@ -371,16 +373,16 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -507,7 +509,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -700,6 +709,12 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -752,7 +767,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -846,6 +860,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -862,6 +881,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + bool system_checked = false; while (!thread_should_exit) { @@ -903,6 +923,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ @@ -915,6 +938,15 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } + /* Perform system checks (again) once params are loaded and MAVLink is up. */ + if (!system_checked && mavlink_fd && + (telemetry.heartbeat_time > 0) && + (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { + + (void)rc_calibration_check(mavlink_fd); + system_checked = true; + } + orb_check(sp_man_sub, &updated); if (updated) { @@ -945,6 +977,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ @@ -1254,12 +1306,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("#audio: NOT ARMING: Press safety switch first."); - } else if (status.main_state != MAIN_STATE_MANUAL) { + /* we check outside of the transition function here because the requirement + * for being in manual mode only applies to manual arming actions. + * the system can be armed in auto if armed via the GCS. + */ + if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); - } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1289,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + tune_negative(true); } /* evaluate the main state machine according to mode switches */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 940a04aa1..80e6861f6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -209,12 +209,18 @@ int led_init() /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); + /* switch blue off */ + led_off(LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; } + /* switch amber off */ + led_off(LED_AMBER); + /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 74885abf6..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,20 +46,32 @@ #include <dirent.h> #include <fcntl.h> #include <string.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/differential_pressure.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_airspeed.h> #include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -98,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - /* - * Perform an atomic state update - */ - irqstate_t flags = irqsave(); - transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == status->arming_state) { + if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (new_arming_state == ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; @@ -124,15 +149,44 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press - // Allow if coming from in air restore + + // Do not perform pre-arm checks if coming from in air restore // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == HIL_STATE_OFF) { + + // Fail transition if pre-arm check fails + if (prearm_ret) { + valid_transition = false; + + // Fail transition if we need safety switch press + } else if (safety->safety_switch_available && !safety->safety_off) { + + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + + valid_transition = false; + } + + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + valid_transition = false; + } } - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { @@ -157,17 +211,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } - } - /* end of atomic state update */ - irqrestore(flags); + /* end of atomic state update */ + irqrestore(flags); + } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "Invalid arming transition from %s to %s"; + static const char *errMsg = "INVAL: %s - %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } @@ -539,3 +591,80 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + int ret; + + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_scale < 9.78f || accel_scale > 9.83f) { + mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + } + + if (accel_scale > 30.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } else { + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + + if (!status->is_rotary_wing) { + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + struct differential_pressure_s diff_pres; + + ret = read(fd, &diff_pres, sizeof(diff_pres)); + + if (ret == sizeof(diff_pres)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + } + +system_eval: + close(fd); + return ret; +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..deaaf55fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 43acee96f..026a4d6c9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -474,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -const mavlink_channel_t +mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -2109,7 +2109,7 @@ Mavlink::task_main(int argc, char *argv[]) write_ptr = (uint8_t*)&msg; // Pull a single message from the buffer - int read_count = available; + size_t read_count = available; if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5f4117ae5..f94036a17 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -213,15 +213,15 @@ public: */ int enable_flow_control(bool enabled); - const mavlink_channel_t get_channel(); + mavlink_channel_t get_channel(); - void configure_stream_threadsafe(const char *stream_name, const float rate); + void configure_stream_threadsafe(const char *stream_name, float rate); bool _task_should_exit; /**< if true, mavlink task should exit */ int get_mavlink_fd() { return _mavlink_fd; } - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } /* Functions for waiting to start transmission until message received. */ @@ -311,15 +311,15 @@ private: pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - bool _param_initialized; param_t _param_system_id; param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e1ebc16cc..667be87b7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -45,7 +45,6 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> @@ -678,7 +677,7 @@ protected: cm_uint16_from_m_float(gps.epv), gps.vel_m_s * 100.0f, _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_visible); + gps.satellites_used); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bb977d277..6d361052c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -751,9 +750,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index ed6776e5c..7279206db 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -44,10 +44,10 @@ #include "mavlink_main.h" MavlinkStream::MavlinkStream() : - _last_sent(0), + next(nullptr), _channel(MAVLINK_COMM_0), _interval(1000000), - next(nullptr) + _last_sent(0) { } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 20e016da3..19c10198c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -72,6 +72,7 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> +#include <systemlib/circuit_breaker.h> #include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> @@ -123,6 +124,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ @@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _actuators_0_circuit_breaker_enabled(false), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + return OK; } @@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main() _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c24c172af..bcc3e2ae7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -466,7 +466,7 @@ MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { double lat_sp, lon_sp; - float alt_sp; + float alt_sp = 0.0f; if (_ref_timestamp != 0) { /* calculate current position setpoint in global frame */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index cbf7ac987..a1c6f09d0 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -42,6 +42,8 @@ #include <string.h> #include <stdlib.h> #include <unistd.h> +#include <math.h> +#include <float.h> #include <systemlib/err.h> #include <geo/geo.h> diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6d7afcf4b..142a73409 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -222,7 +222,7 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 9870ebd05..05eae047c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -49,6 +49,7 @@ #include <sys/prctl.h> #include <termios.h> #include <math.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/actuator_controls.h> @@ -477,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) { + if ((flow.ground_distance_m > 0.31f) && + (flow.ground_distance_m < 4.0f) && + (att.R[2][2] > 0.7f) && + (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -646,25 +651,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* update baro offset */ - baro_offset -= z_est[0]; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; - z_est[0] = 0.0f; local_pos.ref_lat = lat; local_pos.ref_lon = lon; - local_pos.ref_alt = alt; + local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); } } @@ -913,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } + } else { + /* gradually reset xy velocity estimates */ + inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } /* detect land */ @@ -928,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } + /* reset xy velocity estimates when landed */ + x_est[1] = 0.0f; + y_est[1] = 0.0f; } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { @@ -952,11 +961,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", - accel_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt, - flow_updates / updates_dt); + (double)(accel_updates / updates_dt), + (double)(baro_updates / updates_dt), + (double)(gps_updates / updates_dt), + (double)(attitude_updates / updates_dt), + (double)(flow_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index d88419c25..0581f8236 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); @@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index df1cfaa71..423f0d879 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -47,6 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; + float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_flow; + param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 62e6b12cb..185cb20dd 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -47,8 +47,8 @@ #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 5000 -#define RC_CHANNEL_LOW_THRESH -5000 +#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ +#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 79b6546b3..76762c0fc 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -331,6 +331,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } +#ifdef DEBUG static void i2c_dump(void) { @@ -339,3 +340,4 @@ i2c_dump(void) debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); } +#endif diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..70ccab180 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,15 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + char a = 'A'; + write(sbus_fd, &a, 1); } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + char b = 'B'; + write(sbus_fd, &b, 1); } bool diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e483b35e8..39e5b6c41 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/satellite_info.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; } buf; @@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1066,11 +1072,14 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; + /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } @@ -1128,14 +1137,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { - float snr_mean = 0.0f; - - for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { - snr_mean += buf_gps_pos.satellite_snr[i]; - } - - snr_mean /= buf_gps_pos.satellites_visible; - log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1148,44 +1149,55 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; - log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; log_msg.body.log_GPS.snr_mean = snr_mean; log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); + } + + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { - if (_extended_logging) { /* log the SNR of each satellite for a detailed view of signal quality */ - unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); log_msg.msg_type = LOG_GS0A_MSG; memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); - /* fill set A */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + snr_mean = 0.0f; - int satindex = buf_gps_pos.satellite_prn[i] - 1; + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { + + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; log_msg.msg_type = LOG_GS0B_MSG; memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ - for (unsigned i = 0; i < gps_msg_max_snr; i++) { + for (unsigned i = 0; i < sat_info_count; i++) { /* get second bank of satellites, thus deduct bank size from index */ - int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; /* handles index exceeding and wraps to to arithmetic errors */ if ((satindex >= 0) && (satindex < (int)log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); @@ -1394,8 +1406,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.channel_count = buf.rc.chan_count; + memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8908adf4c..3307354a0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1223,9 +1223,9 @@ Sensors::parameter_update_poll(bool forced) } #if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); - printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100)); + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]); + printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100)); fflush(stdout); usleep(5000); #endif @@ -1315,7 +1315,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor @@ -1355,7 +1355,7 @@ float Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) { if (_rc.function[func] >= 0) { - float value = _rc.chan[_rc.function[func]].scaled; + float value = _rc.channels[_rc.function[func]]; if (value < min_value) { return min_value; @@ -1376,7 +1376,7 @@ switch_pos_t Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) { if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1397,7 +1397,7 @@ switch_pos_t Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1489,25 +1489,25 @@ Sensors::rc_poll() * DO NOT REMOVE OR ALTER STEP 1! */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + _rc.channels[i] = 0.0f; } - _rc.chan[i].scaled *= _parameters.rev[i]; + _rc.channels[i] *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) { - _rc.chan[i].scaled = 0.0f; + if (!isfinite(_rc.channels[i])) { + _rc.channels[i] = 0.0f; } } - _rc.chan_count = rc_input.channel_count; + _rc.channel_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c new file mode 100644 index 000000000..8f697749e --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include <systemlib/param/param.h> +#include <systemlib/circuit_breaker.h> + +/** + * Circuit breaker for power supply check + * + * Setting this parameter to 894281 will disable the power valid + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 894281 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); + +/** + * Circuit breaker for rate controller output + * + * Setting this parameter to 140253 will disable the rate + * controller uORB publication. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 140253 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); + +/** + * Circuit breaker for IO safety + * + * Setting this parameter to 894281 will disable IO safety. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 22027 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)param_get(param_find(breaker), &val); + + return (val == magic); +} + diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h new file mode 100644 index 000000000..1175dbce8 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.h + * + * Circuit breaker functionality. + */ + +#ifndef CIRCUIT_BREAKER_H_ +#define CIRCUIT_BREAKER_H_ + +/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING + * + * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE, + * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS. + * http://pixhawk.org/dev/circuit_breakers + * + * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE + * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT. + */ +#define CBRK_SUPPLY_CHK_KEY 894281 +#define CBRK_RATE_CTRL_KEY 140253 +#define CBRK_IO_SAFETY_KEY 22027 + +#include <stdbool.h> + +__BEGIN_DECLS + +__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); + +__END_DECLS + +#endif /* CIRCUIT_BREAKER_H_ */ diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 6c0e876d1..998b5ac7d 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args) fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC lowsyslog("%s: ", getprogname()); - lowvyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 3953b757d..147903aa0 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -52,5 +52,6 @@ SRCS = err.c \ rc_check.c \ otp.c \ board_serial.c \ - pwm_limit/pwm_limit.c + pwm_limit/pwm_limit.c \ + circuit_breaker.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index c0c1a5cb4..b35b333af 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -42,6 +42,7 @@ #include <stdio.h> #include <fcntl.h> +#include <systemlib/err.h> #include <systemlib/rc_check.h> #include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> @@ -98,32 +99,32 @@ int rc_calibration_check(int mavlink_fd) { /* assert min..center..max ordering */ if (param_min < 500) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > 2500) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -139,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) { /* sanity checks pass, enable channel */ if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 57a751e1c..9fff3eb88 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -64,6 +64,9 @@ systemreset(bool to_bootloader) *(uint32_t *)0x40002850 = 0xb007b007; } up_systemreset(); + + /* lock up here */ + while(true); } static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..44b6debc7 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -40,6 +40,7 @@ #include "topics/parameter_update.h" #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" +#include "topics/satellite_info.h" #include "topics/sensor_combined.h" #include "topics/vehicle_attitude.h" #include "topics/vehicle_global_position.h" @@ -88,6 +89,7 @@ T Subscription<T>::getData() { template class __EXPORT Subscription<parameter_update_s>; template class __EXPORT Subscription<actuator_controls_s>; template class __EXPORT Subscription<vehicle_gps_position_s>; +template class __EXPORT Subscription<satellite_info_s>; template class __EXPORT Subscription<sensor_combined_s>; template class __EXPORT Subscription<vehicle_attitude_s>; template class __EXPORT Subscription<vehicle_global_position_s>; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7c3bb0009..687fc1d4a 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/satellite_info.h" +ORB_DEFINE(satellite_info, struct satellite_info_s); + #include "topics/home_position.h" ORB_DEFINE(home_position, struct home_position_s); diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 370c54442..829d8e57d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,59 +43,43 @@ #include "../uORB.h" /** - * The number of RC channel inputs supported. - * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 16. - * This number can be greater then number of RC channels, - * because single RC channel can be mapped to multiple - * functions, e.g. for various mode switches. - */ -#define RC_CHANNELS_MAPPED_MAX 16 - -/** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of - * the channel array chan[]. + * the channel array channels[]. */ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - POSCTL = 6, - LOITER = 7, - OFFBOARD_MODE = 8, - ACRO = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ + ROLL, + PITCH, + YAW, + MODE, + RETURN, + POSCTL, + LOITER, + OFFBOARD_MODE, + ACRO, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, + RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ }; /** * @addtogroup topics * @{ */ - struct rc_channels_s { - - uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ - - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + uint64_t timestamp; /**< Timestamp in microseconds since boot time */ + uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ + float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ + uint8_t channel_count; /**< Number of valid channels */ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ + int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ + uint8_t rssi; /**< Receive signal strength index */ + bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..37c2faa96 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file satellite_info.h + * Definition of the GNSS satellite info uORB topic. + */ + +#ifndef TOPIC_SAT_INFO_H_ +#define TOPIC_SAT_INFO_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ + +#define SAT_INFO_MAX_SATELLITES 20 + +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite info */ + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ +}; + +/** + * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs + * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf + * + * GPS 1-32 + * SBAS 120-158 + * Galileo 211-246 + * BeiDou 159-163, 33-64 + * QZSS 193-197 + * GLONASS 65-96, 255 + * + */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(satellite_info); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index bbacb733a..80d65cd69 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -61,7 +61,6 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ float c_variance_rad; /**< course accuracy estimate rad */ 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. */ @@ -82,14 +81,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + uint8_t satellites_used; /**< Number of satellites used */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 5dc46dacb..56590047f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -191,6 +191,8 @@ struct vehicle_status_s { bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ + bool condition_power_input_valid; /**< set if input power is valid */ + float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ @@ -220,6 +222,8 @@ struct vehicle_status_s { uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + bool circuit_breaker_engaged_power_check; }; /** diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index a3f9dffff..72200f418 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -237,8 +237,10 @@ void at24c_test(void) } else if (result != 1) { vdbg("unexpected %u\n", result); } - if ((count % 100) == 0) + + if ((count % 100) == 0) { vdbg("test %u errors %u\n", count, errors); + } } } diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 7d9484d3e..fca1798e6 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[]) printf("Usage: nshterm <device>\n"); exit(1); } - uint8_t retries = 0; + unsigned retries = 0; int fd = -1; /* try the first 30 seconds */ diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 328205e29..f8bff2f6f 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -64,6 +64,7 @@ static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); static void do_reset(void); +static void do_reset_nostart(void); int param_main(int argc, char *argv[]) @@ -142,6 +143,10 @@ param_main(int argc, char *argv[]) if (!strcmp(argv[1], "reset")) { do_reset(); } + + if (!strcmp(argv[1], "reset_nostart")) { + do_reset_nostart(); + } } errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); @@ -427,3 +432,26 @@ do_reset(void) exit(0); } } + +static void +do_reset_nostart(void) +{ + + int32_t autostart; + int32_t autoconfig; + + (void)param_get(param_find("SYS_AUTOSTART"), &autostart); + (void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig); + + param_reset_all(); + + (void)param_set(param_find("SYS_AUTOSTART"), &autostart); + (void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig); + + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } +} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 1828c660f..e0e6ca537 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -515,7 +515,8 @@ pwm_main(int argc, char *argv[]) ret = poll(&fds, 1, 0); if (ret > 0) { - int ret = read(0, &c, 1); + ret = read(0, &c, 1); + if (ret > 0) { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 0ecdc8eb9..fda949c61 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 86f23b485..570583dee 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -149,6 +149,8 @@ test_file(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -192,7 +194,6 @@ test_file(int argc, char *argv[]) warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index e33cc6ef7..bb8b6c7f5 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -68,7 +68,7 @@ int test_float(int argc, char *argv[]) float sinf_one = sinf(1.0f); float sqrt_two = sqrt(2.0f); - if (sinf_zero == 0.0f) { + if (fabsf(sinf_zero) < FLT_EPSILON) { printf("\t success: sinf(0.0f) == 0.0f\n"); } else { @@ -136,7 +136,7 @@ int test_float(int argc, char *argv[]) ret = -2; } - if (sqrt_two == 1.41421356f) { + if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) { printf("\t success: sqrt(2.0f) == 1.41421f\n"); } else { @@ -188,7 +188,7 @@ int test_float(int argc, char *argv[]) double d1d2 = d1 * d2; - if (d1d2 == 2.022200000000000219557705349871) { + if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) { printf("\t success: 1.0111 * 2.0 == 2.0222\n"); } else { @@ -201,7 +201,7 @@ int test_float(int argc, char *argv[]) // Assign value of f1 to d1 d1 = f1; - if (f1 == (float)d1) { + if (fabsf(f1 - (float)d1) < FLT_EPSILON) { printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { @@ -216,7 +216,7 @@ int test_float(int argc, char *argv[]) double sin_one = sin(1.0); double atan2_ones = atan2(1.0, 1.0); - if (sin_zero == 0.0) { + if (fabs(sin_zero - 0.0) < DBL_EPSILON) { printf("\t success: sin(0.0) == 0.0\n"); } else { @@ -224,7 +224,7 @@ int test_float(int argc, char *argv[]) ret = -9; } - if (sin_one == 0.841470984807896504875657228695) { + if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) { printf("\t success: sin(1.0) == 0.84147098480\n"); } else { @@ -232,7 +232,7 @@ int test_float(int argc, char *argv[]) ret = -10; } - if (atan2_ones != 0.785398) { + if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) { printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); } else { diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index cdb4362ba..43231ccad 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -171,6 +171,8 @@ test_mtd(int argc, char *argv[]) } end = hrt_absolute_time(); + warnx("write took %llu us", (end - start)); + close(fd); fd = open(PARAM_FILE_NAME, O_RDONLY); |