From 559c62b6bc4923854e106be5987db92197e0bae1 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Fri, 9 May 2014 15:17:38 -0700 Subject: Changed low threshold in px4io firmware to 10% to ensure compatibility with user configured single channel, mode switches --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 56c5aa005..a8c560d40 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); -- cgit v1.2.3 From e92620b9b2d130c5ea3e9fabb91a9e74cba4f710 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 23:15:58 +0200 Subject: rc_channels topic cleanup --- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/rc_channels.h | 68 ++++++++++++-------------------- 4 files changed, 41 insertions(+), 58 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..401faf8a7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -45,7 +45,6 @@ #include #include -#include #include #include #include diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 70ce76806..b29c0e086 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1200,8 +1200,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 b268b1b36..319626d6e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1202,9 +1202,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 @@ -1334,7 +1334,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; @@ -1355,7 +1355,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; @@ -1376,7 +1376,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; @@ -1468,25 +1468,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/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 @@ -42,60 +42,44 @@ #include #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. */ /** -- cgit v1.2.3 From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/commander.cpp | 27 ++++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 22 +++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 51 insertions(+) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..55c74fdff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -717,6 +718,9 @@ 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; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -853,6 +857,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 */ @@ -933,6 +942,24 @@ 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; + 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 */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..818974648 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ea20a317a..2ed0d6ad4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -179,6 +179,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 */ -- cgit v1.2.3 From 032fe389a5630a22c9da4642d79b3fae05a213f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:11 +0200 Subject: systemlib: Add circuit breakers --- src/modules/systemlib/circuit_breaker.c | 93 +++++++++++++++++++++++++++++++++ src/modules/systemlib/circuit_breaker.h | 55 +++++++++++++++++++ src/modules/systemlib/module.mk | 5 +- 3 files changed, 151 insertions(+), 2 deletions(-) create mode 100644 src/modules/systemlib/circuit_breaker.c create mode 100644 src/modules/systemlib/circuit_breaker.h (limited to 'src') diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c new file mode 100644 index 000000000..b59531d69 --- /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 +#include + +/** + * 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..86439e2a5 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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_ + +#define CBRK_SUPPLY_CHK_KEY 894281 +#define CBRK_RATE_CTRL_KEY 140253 +#define CBRK_IO_SAFETY_KEY 22027 + +#include + +__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/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 -- cgit v1.2.3 From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++++ src/modules/commander/state_machine_helper.cpp | 32 +++++++++++++++----------- 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff..71067ac4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) 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); @@ -907,6 +911,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 */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648..84f4d03af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // 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) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", 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) { -- cgit v1.2.3 From 55ad5588a88bc4957583067beeffe20aa2fcd442 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:16 +0200 Subject: system status topic: Add support for circuit breaker --- src/modules/uORB/topics/vehicle_status.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2ed0d6ad4..85962883d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -208,6 +208,8 @@ struct vehicle_status_s { uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + bool circuit_breaker_engaged_power_check; }; /** -- cgit v1.2.3 From 77365188ada556c5953f6f026566d4912e8c6e76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:37 +0200 Subject: mc_att_control: Support circuit breakers --- src/modules/mc_att_control/mc_att_control_main.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) (limited to 'src') 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 #include #include +#include #include #include @@ -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); + } } } } -- cgit v1.2.3 From 8ea5fd20c1c268c6baf145606186592bf5bd3699 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:36:05 +0200 Subject: px4io driver: Add support for circuit breakers --- src/drivers/px4io/px4io.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 972f45148..c6acfe6be 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include @@ -1010,6 +1011,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); + } + } + } } -- cgit v1.2.3 From 16ca6c1605e0864d37cce4cfdbe790df5746bb38 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 May 2014 14:52:42 +0200 Subject: position_estimator_inav: don't change local z on first time ref initialization --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) (limited to 'src') 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 fdc3233e0..f236d546b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -622,8 +622,6 @@ 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; @@ -631,18 +629,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; - z_est[0] = 0.0f; y_est[2] = accel_NED[1]; 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", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", lat, lon, alt); } } -- cgit v1.2.3 From 6df2fe9aebb008932055b85f134908127d8b3931 Mon Sep 17 00:00:00 2001 From: Kynos Date: Thu, 29 May 2014 21:23:57 +0200 Subject: U-blox driver rework, step 1 Handle u-blox 7+8 GNSS, too. Disabled NAV-SVINFO for now (will be rewritten as an optional feature in a later step). Reduced parser buffer size from 300 to 80. --- src/drivers/gps/ubx.cpp | 37 ++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 55 ++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 74 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..ee53e2ec4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,12 +219,14 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_ENABLE_NAV_SVINFO 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"); return 1; } +#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -386,7 +388,6 @@ UBX::parse_char(uint8_t b) 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 } else { @@ -399,7 +400,7 @@ UBX::parse_char(uint8_t b) _rx_count++; } else { - warnx("buffer full"); + warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); decode_init(); return -1; } @@ -450,6 +451,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); + _gps_position->satellites_visible = packet->numSV; ret = 1; break; @@ -486,6 +488,7 @@ UBX::handle_message() break; } +#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); const int length_part1 = 8; @@ -527,7 +530,9 @@ UBX::handle_message() _gps_position->satellite_azimuth[i] = 0; } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ + /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones + /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ if (packet_part1->numCh > 0) { _gps_position->satellite_info_available = true; @@ -541,6 +546,7 @@ UBX::handle_message() ret = 1; break; } +#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +579,34 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { + case UBX_CLASS_MON: switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + case UBX_MESSAGE_MON_HW: + switch (_payload_size) { - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; + case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; + ret = 1; + break; + case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ + _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; + _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; ret = 1; break; + + default: // unexpected payload size: + ret = 1; // ignore but don't disable msg + break; } + break; default: break; } - } + break; default: break; @@ -597,7 +614,7 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); hrt_abstime t = hrt_absolute_time(); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..56f73fa63 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,6 +48,10 @@ #include "gps_helper.h" +/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#undef UBX_ENABLE_NAV_SVINFO + #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -60,10 +64,8 @@ /* 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 @@ -72,9 +74,22 @@ #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_MESSAGE_CFG_NAV5 0x24 - #define UBX_MESSAGE_MON_HW 0x09 +/* Rx msg payload sizes */ +#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ +#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ +#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ +#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ +#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ +#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ +#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ + +/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ + +/* CFG class Tx msg defs */ #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -87,7 +102,6 @@ #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 */ @@ -98,7 +112,6 @@ #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 */ @@ -140,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -213,7 +226,7 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_velned_packet_t; -struct gps_bin_mon_hw_packet { +struct gps_bin_mon_hw_ubx6_packet { uint32_t pinSel; uint32_t pinBank; uint32_t pinDir; @@ -233,6 +246,25 @@ struct gps_bin_mon_hw_packet { uint32_t pullL; }; +struct gps_bin_mon_hw_ubx7_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[17]; + 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 */ @@ -343,7 +375,14 @@ typedef enum { //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) -#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer +/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ +/* TODO: make this a command line option, allocate buffer dynamically */ +#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ +#ifdef UBX_ENABLE_NAV_SVINFO + #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#else + #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) +#endif class UBX : public GPS_Helper { -- cgit v1.2.3 From 7349fa90376714b00fe920538950b31a46169d60 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 11:03:16 +0200 Subject: Beautifications for tab size 8 Beautifications for tab size 8 --- src/drivers/gps/ubx.cpp | 6 ++-- src/drivers/gps/ubx.h | 74 ++++++++++++++++++++++++------------------------- 2 files changed, 40 insertions(+), 40 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ee53e2ec4..d7e9454ae 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -59,10 +59,10 @@ #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 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 56f73fa63..b844c315e 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -34,7 +34,7 @@ /** * @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 @@ -86,7 +86,7 @@ #define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ #define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ /* CFG class Tx msg defs */ @@ -127,21 +127,21 @@ struct ubx_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 */ + 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; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + 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 */ + 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; @@ -153,7 +153,7 @@ typedef struct { uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; - uint8_t numSV; /**< Number of SVs used in Nav Solution */ + uint8_t numSV; /**< Number of SVs used in Nav Solution */ uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; @@ -163,47 +163,47 @@ 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) */ + 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) */ +// 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; typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint8_t numCh; /**< Number of channels */ + 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; typedef struct { - uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ - uint8_t svid; /**< Satellite ID */ + 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 */ + 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; @@ -268,8 +268,8 @@ struct gps_bin_mon_hw_ubx7_packet { //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 */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -395,7 +395,7 @@ public: private: /** - * Parse the binary MTK packet + * Parse the binary UBX packet */ int parse_char(uint8_t b); -- cgit v1.2.3 From 9bad828bc0033c7017978de2321d3a8698c0afc6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 30 May 2014 14:30:25 +0200 Subject: U-blox driver rework, step 2 Moved satellite info from vehicle_gps_position_s into a new uORB topic satellite_info. Renamed satellites_visible to satellites_used to reflect true content. sdlog2 will log info for GPS satellites only for now. --- src/drivers/blinkm/blinkm.cpp | 8 +-- src/drivers/gps/gps.cpp | 2 +- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/ubx.cpp | 3 +- src/drivers/hott/messages.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sdlog2/sdlog2.c | 48 ++++++++------ src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/satellite_info.h | 86 ++++++++++++++++++++++++++ src/modules/uORB/topics/vehicle_gps_position.h | 9 +-- 12 files changed, 130 insertions(+), 40 deletions(-) create mode 100644 src/modules/uORB/topics/satellite_info.h (limited to 'src') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..39bf1fcfc 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; satloopget_position_update_rate()); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 680f00d97..7b957e55d 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_m = _gps_position->eph_m; // 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 d7e9454ae..462875b6c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include "ubx.h" @@ -451,7 +452,7 @@ UBX::handle_message() _gps_position->s_variance_m_s = packet->sAcc; _gps_position->p_variance_m = packet->pAcc; _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_visible = packet->numSV; + _gps_position->satellites_used = packet->numSV; ret = 1; break; 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/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f56..146e2d4b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -548,7 +548,7 @@ protected: cm_uint16_from_m_float(gps->epv_m), 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 53769e0cf..46da1a0be 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -682,9 +682,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/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..4ff6c30aa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -139,6 +140,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 */ @@ -941,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct estimator_status_report estimator_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; } buf; memset(&buf, 0, sizeof(buf)); @@ -1000,6 +1004,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; @@ -1017,6 +1022,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)); @@ -1053,6 +1059,9 @@ 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 */ @@ -1115,14 +1124,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; @@ -1135,44 +1136,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); 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::getData() { template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..fc12b9ed5 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/satellite_info.h b/src/modules/uORB/topics/satellite_info.h new file mode 100644 index 000000000..5f799eda4 --- /dev/null +++ b/src/modules/uORB/topics/satellite_info.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * + * 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 +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GNSS Satellite Info. + */ +struct satellite_info_s { + uint64_t timestamp; /**< Timestamp of satellite information */ + uint8_t count; /**< Number of satellites in satellite_...[] arrays */ + uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[20]; /**< 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 5924a324d..a50f02139 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -82,14 +82,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 */ }; /** -- cgit v1.2.3 From 9f754e0e9a2e2ad75a3fe5a15690cf542ce08e8a Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 14:05:35 +0200 Subject: U-blox driver rework, step 3 More sat info isolation Flush input after changing baudrate to allow driver restart w/o GNSS module restart --- src/drivers/gps/gps.cpp | 98 ++++++++++++++----------- src/drivers/gps/ubx.cpp | 119 ++++++++++++++----------------- src/drivers/gps/ubx.h | 10 +-- src/modules/uORB/topics/satellite_info.h | 17 +++-- 4 files changed, 126 insertions(+), 118 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d2ffc30ba..d8e2bd797 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include @@ -106,8 +107,10 @@ private: 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 + 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 _report_sat_info; ///< 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 @@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), - _report_pub(-1), + _report_gps_pos_pub(-1), + _report_sat_info_pub(-1), _rate(0.0f), _fake_gps(fake_gps) { @@ -172,7 +176,8 @@ 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)); + memset(&_report_sat_info, 0, sizeof(_report_sat_info)); _debug_enabled = true; } @@ -271,33 +276,33 @@ 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_m = 0.9f; - _report.epv_m = 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.p_variance_m = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph_m = 0.9f; + _report_gps_pos.epv_m = 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 +318,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, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); break; case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); + _Helper = new MTK(_serial_fd, &_report_gps_pos); break; default: @@ -332,20 +337,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 (helper_ret & 2) { + if (_report_sat_info_pub > 0) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_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) { @@ -357,7 +375,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -447,11 +465,11 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - if (_report.timestamp_position != 0) { - warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); - warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); + 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_m, (double)_report_gps_pos.epv_m); 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); @@ -578,7 +596,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 462875b6c..dd47572b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -65,9 +65,13 @@ #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 -UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) : _fd(fd), _gps_position(gps_position), + _satellite_info(satellite_info), + _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), _disable_cmd_last(0) @@ -84,14 +88,19 @@ 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(); + wait_for_ack(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 */ @@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate) 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.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; @@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate) 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.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; @@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate) break; } - if (baud_i >= 5) { + if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { return 1; } @@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate) return 1; } -#ifdef UBX_ENABLE_NAV_SVINFO - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + if (_enable_sat_info) { + 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"); - return 1; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } } -#endif configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); @@ -244,14 +253,13 @@ int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - uint64_t time_started = hrt_absolute_time(); + uint64_t time_end = hrt_absolute_time() + timeout * 1000; - while (hrt_absolute_time() < time_started + timeout * 1000) { + while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { if (receive(timeout) > 0) { if (!_waiting_for_ack) { return 1; } - } else { return -1; // timeout or error receiving, or NAK } @@ -275,7 +283,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool handled = false; + int handled = 0; while (true) { @@ -290,7 +298,7 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ if (handled) { - return 1; + return handled; } else { return -1; @@ -311,8 +319,7 @@ 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 |= handle_message(); } } } @@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_UNINIT: if (b == UBX_SYNC1) { _decode_state = UBX_DECODE_GOT_SYNC1; + //### printf("\nA"); } break; @@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b) case UBX_DECODE_GOT_SYNC1: if (b == UBX_SYNC2) { _decode_state = UBX_DECODE_GOT_SYNC2; + //### printf("B"); } else { /* Second start symbol was wrong, reset state machine */ decode_init(); /* don't return error, it can be just false sync1 */ + //### printf("-"); } break; @@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b) add_byte_to_checksum(b); _message_class = b; _decode_state = UBX_DECODE_GOT_CLASS; + //### printf("C"); break; case UBX_DECODE_GOT_CLASS: add_byte_to_checksum(b); _message_id = b; _decode_state = UBX_DECODE_GOT_MESSAGEID; + //### printf("D"); break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; + //### printf("E"); break; case UBX_DECODE_GOT_LENGTH1: add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; + //### printf("F"); break; case UBX_DECODE_GOT_LENGTH2: @@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b) 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]) { + //### printf("O\n"); return 1; // message received successfully } else { @@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b) } else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; + //### printf("."); } else { warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); @@ -489,65 +505,37 @@ UBX::handle_message() break; } -#ifdef UBX_ENABLE_NAV_SVINFO case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); + if (!_enable_sat_info) { // if satellite info processing not enabled: + break; // ignore and disable NAV-SVINFO message + } 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]); + for (_satellite_info->count = 0; + _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); + _satellite_info->count++) { + /* set pointer to satellite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* 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; + _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; + _satellite_info->snr[_satellite_info->count] = packet_part2->cno; + _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); + _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _satellite_info->svid[_satellite_info->count] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - /* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ - /* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones - /* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */ + _satellite_info->timestamp = hrt_absolute_time(); - 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; + ret = 2; break; } -#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */ case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -650,7 +638,6 @@ UBX::handle_message() warnx("ubx: not acknowledged"); /* configuration obviously not successful */ _waiting_for_ack = false; - ret = -1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index b844c315e..90aeffedc 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -48,9 +48,7 @@ #include "gps_helper.h" -/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#undef UBX_ENABLE_NAV_SVINFO +#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -378,7 +376,7 @@ typedef enum { /* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ /* TODO: make this a command line option, allocate buffer dynamically */ #define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#ifdef UBX_ENABLE_NAV_SVINFO +#if (UBX_ENABLE_NAV_SVINFO != 0) #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #else #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) @@ -387,7 +385,7 @@ typedef enum { 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, const bool &enable_sat_info); ~UBX(); int receive(unsigned timeout); int configure(unsigned &baudrate); @@ -434,6 +432,8 @@ private: 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; uint8_t _message_class_needed; diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h index 5f799eda4..37c2faa96 100644 --- a/src/modules/uORB/topics/satellite_info.h +++ b/src/modules/uORB/topics/satellite_info.h @@ -53,14 +53,17 @@ /** * GNSS Satellite Info. */ + +#define SAT_INFO_MAX_SATELLITES 20 + struct satellite_info_s { - uint64_t timestamp; /**< Timestamp of satellite information */ - uint8_t count; /**< Number of satellites in satellite_...[] arrays */ - uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ - uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ - uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + 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. */ }; /** -- cgit v1.2.3 From 018eeeecad748959a8afa4845a536e897a57249d Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 15:56:42 +0200 Subject: Merged ubx driver part of pull request #1031 --- src/drivers/gps/ubx.cpp | 15 +++++++++++---- src/drivers/gps/ubx.h | 2 ++ 2 files changed, 13 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index dd47572b4..0708e23b4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -74,6 +74,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _enable_sat_info(enable_sat_info), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), _disable_cmd_last(0) { decode_init(); @@ -208,7 +210,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("MSG CFG FAIL: NAV TIMEUTC"); @@ -286,9 +288,10 @@ UBX::receive(unsigned timeout) int handled = 0; while (true) { + 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]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -297,7 +300,9 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; return handled; } else { @@ -456,7 +461,8 @@ UBX::handle_message() _rate_count_lat_lon++; - ret = 1; + _got_posllh = true; + ret = 1; break; } @@ -552,6 +558,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 90aeffedc..50df3ada2 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -436,6 +436,8 @@ private: bool _enable_sat_info; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; -- cgit v1.2.3 From 3538773b99d4231bf8a64e9c257cfe0299a93021 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 16:05:04 +0200 Subject: Initialize rate values Initialize rate values (else invalid rates wil be displayed during the first measurement interval). --- src/drivers/gps/gps_helper.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') 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; }; -- cgit v1.2.3 From 07458b284d4cea48bc3b6e0cb5dcaba1c2c1d4d6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 8 Jun 2014 17:54:50 +0200 Subject: Gps driver: make enable_sat_info a command line option --- src/drivers/gps/gps.cpp | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d8e2bd797..f4d2f4b44 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -83,7 +83,7 @@ static const int ERROR = -1; 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(); @@ -113,6 +113,7 @@ private: orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info float _rate; ///< position update rate bool _fake_gps; ///< fake gps output + bool _enable_sat_info; ///< enable sat info /** @@ -157,7 +158,7 @@ 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), @@ -167,7 +168,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) : _report_gps_pos_pub(-1), _report_sat_info_pub(-1), _rate(0.0f), - _fake_gps(fake_gps) + _fake_gps(fake_gps), + _enable_sat_info(enable_sat_info) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -318,7 +320,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO); + _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -464,6 +466,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, @@ -487,7 +490,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(); @@ -497,7 +500,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; @@ -505,7 +508,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; @@ -598,6 +601,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. @@ -619,7 +623,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")) @@ -644,5 +654,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]"); } -- cgit v1.2.3 From b6b3ad6e1e31503d230ad6e7bc8f2d7b5b596ad7 Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 14:05:47 +0200 Subject: U-blox driver rework,, step 4 Config phase and parser rewrite --- src/drivers/gps/gps.cpp | 41 ++- src/drivers/gps/ubx.cpp | 903 ++++++++++++++++++++++++++---------------------- src/drivers/gps/ubx.h | 662 ++++++++++++++++++----------------- 3 files changed, 849 insertions(+), 757 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f4d2f4b44..6ae325297 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -80,6 +80,14 @@ #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: @@ -103,17 +111,17 @@ private: char _port[20]; ///< device / serial port path 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_gps_pos; ///< uORB topic 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 _report_sat_info; ///< uORB topic for satellite info + 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 - bool _enable_sat_info; ///< enable sat info /** @@ -165,11 +173,12 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), + _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), - _enable_sat_info(enable_sat_info) + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -179,7 +188,13 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) : /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - memset(&_report_sat_info, 0, sizeof(_report_sat_info)); + + /* 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; } @@ -214,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); @@ -320,7 +335,7 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, _enable_sat_info); + _Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -353,12 +368,12 @@ GPS::task_main() _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); } } - if (helper_ret & 2) { + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { - orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info); + 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), &_report_sat_info); + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); } } } @@ -466,7 +481,7 @@ GPS::print_info() } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_enable_sat_info) ? "enabled" : "disabled"); + warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 0708e23b4..1783ae0f7 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 * @author Julian Oes * @author Anton Babushkin * + * @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 @@ -65,18 +69,28 @@ #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 MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) +#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00)) + +/**** 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, const bool &enable_sat_info) : + +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), - _enable_sat_info(enable_sat_info), _configured(false), - _waiting_for_ack(false), + _ack_state(UBX_ACK_IDLE), _got_posllh(false), _got_velned(false), - _disable_cmd_last(0) + _disable_cmd_last(0), + _ack_waiting_msg(0) { decode_init(); } @@ -100,55 +114,41 @@ UBX::configure(unsigned &baudrate) /* flush input and wait for at least 20 ms silence */ decode_init(); - wait_for_ack(20); + 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 */ @@ -156,94 +156,68 @@ UBX::configure(unsigned &baudrate) } if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) { - return 1; + 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)); - - _message_class_needed = UBX_CLASS_CFG; - _message_id_needed = UBX_MESSAGE_CFG_RATE; + /* 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; - 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_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate)); - 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; - - 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; + 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; - send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5)); - 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); + configure_message_rate(UBX_MSG_NAV_POSLLH, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV POSLLH"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 5); + configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV TIMEUTC"); + 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_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SOL"); + 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); + configure_message_rate(UBX_MSG_NAV_VELNED, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV VELNED"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } - if (_enable_sat_info) { - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 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); + configure_message_rate(UBX_MSG_MON_HW, 1); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -251,27 +225,36 @@ UBX::configure(unsigned &baudrate) 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_end = hrt_absolute_time() + timeout * 1000; + int ret = -1; - while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) { - 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]; @@ -295,7 +278,7 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ - warnx("poll error"); + UBX_WARN("ubx poll() err"); return -1; } else if (ret == 0) { @@ -323,433 +306,517 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i]) > 0) { - handled |= handle_message(); - } + 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; - //### printf("\nA"); - } + /* 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; - //### printf("B"); + /* 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 */ - //### printf("-"); } + 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; - //### printf("C"); + _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; - //### printf("D"); + _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; - //### printf("E"); + _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; - //### printf("F"); + if (_rx_msg != UBX_MSG_NAV_SVINFO) { + ret = payload_rx_add(b); // add a payload byte + } else { + ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + } + 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; + + /* 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; + + /* 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; - case UBX_DECODE_GOT_LENGTH2: + default: + break; + } - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); + return ret; +} - _rx_buffer[_rx_count] = b; +/** + * Start payload rx + */ +int // -1 = abort, 0 = continue +UBX::payload_rx_init() +{ + int ret = 0; - /* 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]) { - //### printf("O\n"); - return 1; // message received successfully + _rx_state = UBX_RXMSG_HANDLE; // handle by default - } else { - warnx("checksum wrong"); - decode_init(); - return -1; - } + switch (_rx_msg) { + 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 + break; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - //### printf("."); + 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 + break; - } else { - warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - decode_init(); - return -1; - } + 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 + 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 + break; + + 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; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } -int -UBX::handle_message() + return ret; +} + +/** + * Add svinfo payload rx byte + */ +int // -1 = error, 0 = ok, 1 = payload completed +UBX::payload_rx_add_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_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (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] + ); + } + } + } - 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; + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; - _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->satellites_used = packet->numSV; + return ret; +} - ret = 1; - break; - } +/** + * 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_POSLLH: + UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); + + _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_m = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + + _gps_position->timestamp_position = hrt_absolute_time(); + + _rate_count_lat_lon++; + _got_posllh = true; + + ret = 1; + break; + + case UBX_MSG_NAV_SOL: + UBX_TRACE_RXMSG("Rx NAV-SOL\n"); + + _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; + _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; + _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; + + _gps_position->timestamp_variance = hrt_absolute_time(); + + ret = 1; + break; - 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; + case UBX_MSG_NAV_TIMEUTC: + UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - /* 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); + { + /* 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 = 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_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)(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_timeutc.nano * 1e-3f); + } - ret = 1; - break; - } + _gps_position->timestamp_time = hrt_absolute_time(); - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - if (!_enable_sat_info) { // if satellite info processing not enabled: - break; // ignore and disable NAV-SVINFO message - } - 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; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (_satellite_info->count = 0; - _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0])); - _satellite_info->count++) { - /* set pointer to satellite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]); - - /* write satellite information to global storage */ - _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01; - _satellite_info->snr[_satellite_info->count] = packet_part2->cno; - _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev); - _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _satellite_info->svid[_satellite_info->count] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - _satellite_info->timestamp = hrt_absolute_time(); - - ret = 2; - break; - } + ret = 1; + break; - 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; + case UBX_MSG_NAV_SVINFO: + UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); - _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(); + // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp + _satellite_info->timestamp = hrt_absolute_time(); - _rate_count_vel++; + ret = 2; + break; - _got_velned = true; - ret = 1; - break; - } + case UBX_MSG_NAV_VELNED: + UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); - default: - break; - } + _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; - break; + _gps_position->timestamp_velocity = hrt_absolute_time(); - case UBX_CLASS_ACK: { - /* ignore ACK when already configured */ - ret = 1; - break; - } + _rate_count_vel++; + _got_velned = true; - case UBX_CLASS_MON: - switch (_message_id) { + ret = 1; + break; - case UBX_MESSAGE_MON_HW: - switch (_payload_size) { + case UBX_MSG_MON_HW: + UBX_TRACE_RXMSG("Rx MON-HW\n"); - case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE: /* u-blox 6 msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + switch (_rx_payload_length) { - case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE: /* u-blox 7+ msg format */ - _gps_position->noise_per_ms = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS; - _gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd; - ret = 1; - break; + 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; - default: // unexpected payload size: - ret = 1; // ignore but don't disable msg - break; - } - break; + ret = 1; + break; - default: - break; - } + 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; + + ret = 1; break; - default: + default: // unexpected payload size: + ret = 0; // don't handle message break; } + break; - if (ret == 0) { - /* message not handled */ - warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); - - hrt_abstime t = hrt_absolute_time(); + case UBX_MSG_ACK_ACK: + UBX_TRACE_RXMSG("Rx ACK-ACK\n"); - 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); - } + if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { + _ack_state = UBX_ACK_GOT_ACK; } - } 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; - } + ret = 1; + break; - case UBX_MESSAGE_ACK_NAK: { - // printf("GOT ACK_NAK\n"); - warnx("ubx: not acknowledged"); - /* configuration obviously not successful */ - _waiting_for_ack = false; - break; - } + case UBX_MSG_ACK_NAK: + UBX_TRACE_RXMSG("Rx ACK-NAK\n"); - default: - break; - } + 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) -void -UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) -{ - 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)); + cfg_msg.msg = msg; + cfg_msg.rate = rate; + + send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg)); } void -UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length) { - ssize_t ret = 0; + ubx_header_t header = {UBX_SYNC1, UBX_SYNC2}; + ubx_checksum_t checksum = {0, 0}; - /* calculate the checksum now */ - add_checksum_to_message(packet, length); + // Populate header + header.msg = msg; + header.length = length; - const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + // Calculate checksum + calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + calc_checksum(payload, length, &checksum); - /* start with the two sync bytes */ - ret += write(fd, sync_bytes, sizeof(sync_bytes)); - ret += write(fd, packet, length); - - 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"); -} - -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); + // Send message + write(_fd, (const void *)&header, sizeof(header)); + write(_fd, (const void *)payload, length); + write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 50df3ada2..f084fc655 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 @@ -41,6 +41,9 @@ * @author Julian Oes * @author Anton Babushkin * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * */ #ifndef UBX_H_ @@ -48,346 +51,341 @@ #include "gps_helper.h" -#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */ - #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_SOL 0x06 -#define UBX_MESSAGE_NAV_VELNED 0x12 -#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 - -/* Rx msg payload sizes */ -#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE 28 /**< NAV_POSLLH Rx msg payload size */ -#define UBX_NAV_SOL_RX_PAYLOAD_SIZE 52 /**< NAV_SOL Rx msg payload size */ -#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE 36 /**< NAV_VELNED Rx msg payload size */ -#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE 20 /**< NAV_TIMEUTC Rx msg payload size */ -#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE 68 /**< MON_HW Rx msg payload size for u-blox 6 and below */ -#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE 60 /**< MON_HW Rx msg payload size for u-blox 7 and above */ -#define UBX_MAX_RX_PAYLOAD_SIZE 70 /**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */ +/* 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_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_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_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) /* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE (8 + 12 * UBX_MAX_NUM_SAT) /**< NAV_SVINFO Rx msg payload size */ - -/* CFG class Tx msg defs */ -#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 - - -// ************ -/** the structures of the binary packets */ -#pragma pack(push, 1) +#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ +#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ -struct ubx_header { - uint8_t sync1; - uint8_t sync2; - uint8_t msg_class; - uint8_t msg_id; - uint16_t length; -}; +/* 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 */ -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; +/* 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 */ -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; /**< Number of SVs used in Nav Solution */ - uint32_t reserved2; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_sol_packet_t; +/* 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 */ -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; +/* 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 -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; +/*** u-blox protocol binary message and payload definitions ***/ +#pragma pack(push, 1) +/* General: Header */ 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; + uint8_t sync1; + uint8_t sync2; + uint16_t msg; + uint16_t length; +} ubx_header_t; +/* General: Checksum */ typedef struct { - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_svinfo_part3_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 - 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_ubx6_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; -}; - -struct gps_bin_mon_hw_ubx7_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[17]; - 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] */ + 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 { - uint8_t clsID; - uint8_t msgID; - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_ack_ack_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-TIMEUTC */ 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] */ + 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 documentation) */ +} ubx_payload_rx_nav_timeutc_t; + +/* Rx NAV-SVINFO Part 1 */ 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 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; - 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 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; - 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; - + 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 msgClass_payload; - uint8_t msgID_payload; - uint8_t rate; - uint8_t ck_a; - uint8_t ck_b; -} type_gps_bin_cfg_msg_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 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; -struct ubx_cfg_msg_rate { - uint8_t msg_class; - uint8_t msg_id; +/* 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 { + union { + uint16_t msg; + struct { + uint8_t msgClass; + uint8_t msgID; + }; + }; uint8_t rate; -}; +} ubx_payload_tx_cfg_msg_t; + +/* General message and payload buffer union */ +typedef union { + 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_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; +#pragma pack(pop) +/*** END OF u-blox protocol binary message and payload definitions ***/ -// END the structures of the binary packets -// ************ - +/* 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; -/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ -/* TODO: make this a command line option, allocate buffer dynamically */ -#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ -#if (UBX_ENABLE_NAV_SVINFO != 0) - #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#else - #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) -#endif class UBX : public GPS_Helper { public: - UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info); + 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: @@ -395,12 +393,23 @@ private: /** * Parse the binary UBX packet */ - int parse_char(uint8_t b); + int parse_char(const uint8_t b); + + /** + * Start payload rx + */ + int payload_rx_init(void); + + /** + * Add payload rx byte + */ + int payload_rx_add(const uint8_t b); + int payload_rx_add_svinfo(const uint8_t b); /** - * Handle the package once it has arrived + * Finish payload rx */ - int handle_message(void); + int payload_rx_done(void); /** * Reset the parse state machine for a fresh start @@ -410,45 +419,46 @@ 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(uint8_t msg_class, uint8_t msg_id, uint8_t rate); + void configure_message_rate(const uint16_t msg, const uint8_t rate); - void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - - void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + /** + * Calculate & add checksum for given buffer + */ + void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum); - int wait_for_ack(unsigned timeout); + /** + * Wait for message acknowledge + */ + int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); 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; - 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; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 7724e3d889eecd6733c6e85341a9f0b97faa37cf Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 13 Jun 2014 23:18:58 +0200 Subject: Remove unreferenced SVINFO length defines --- src/drivers/gps/ubx.h | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f084fc655..8fd8d4da6 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -88,10 +88,6 @@ #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) -/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */ -#define UBX_MAX_NUM_SAT_SVINFO 50 /**< Practical observed max number of satellites in SVNFO msg */ -#define UBX_PAYLOAD_NAV_SVINFO (8 + 12 * UBX_MAX_NUM_SAT_SVINFO) /**< NAV_SVINFO Rx msg payload size */ - /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -- cgit v1.2.3 From 1b8e72be1b9614bff4e1bdf81ad7f3265754e062 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sun, 15 Jun 2014 17:26:14 +0200 Subject: Merge sAcc & pAcc scaling fix from branch inav_gps_delay --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 1783ae0f7..56e7a8bd7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -643,8 +643,8 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-SOL\n"); _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; - _gps_position->s_variance_m_s = _buf.payload_rx_nav_sol.sAcc; - _gps_position->p_variance_m = _buf.payload_rx_nav_sol.pAcc; + _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m + _gps_position->p_variance_m = (float)_buf.payload_rx_nav_sol.pAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); -- cgit v1.2.3 From 243db01a37d5d3adecc2e010ac4d9819cf3d6c37 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 01:43:44 +0200 Subject: Request and display MON-VER message at startup --- src/drivers/gps/ubx.cpp | 73 +++++++++++++++++++++++++++++++++++++++++++------ src/drivers/gps/ubx.h | 21 ++++++++++++-- 2 files changed, 84 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 56e7a8bd7..be19ce488 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -93,6 +93,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _ack_waiting_msg(0) { decode_init(); + memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); + memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -221,6 +223,9 @@ UBX::configure(unsigned &baudrate) 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; } @@ -385,10 +390,16 @@ UBX::parse_char(const uint8_t b) case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); - if (_rx_msg != UBX_MSG_NAV_SVINFO) { - ret = payload_rx_add(b); // add a payload byte - } else { - ret = payload_rx_add_svinfo(b); // add a svinfo payload byte + 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 @@ -477,6 +488,9 @@ UBX::payload_rx_init() _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured 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 */ @@ -559,10 +573,10 @@ UBX::payload_rx_add(const uint8_t b) } /** - * Add svinfo payload rx byte + * Add NAV-SVINFO payload rx byte */ int // -1 = error, 0 = ok, 1 = payload completed -UBX::payload_rx_add_svinfo(const uint8_t b) +UBX::payload_rx_add_nav_svinfo(const uint8_t b) { int ret = 0; @@ -606,6 +620,41 @@ UBX::payload_rx_add_svinfo(const uint8_t b) return ret; } +/** + * 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; + + 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 + strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); + strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); + UBX_WARN("VER sw %30s", _ubx_sw_version); + UBX_WARN("VER hw %10s", _ubx_hw_version); + } + // 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); + } + } + + if (++_rx_payload_index >= _rx_payload_length) { + ret = 1; // payload received completely + } + + return ret; +} + /** * Finish payload rx */ @@ -712,6 +761,12 @@ UBX::payload_rx_done(void) 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"); @@ -813,10 +868,12 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len // Calculate checksum calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - calc_checksum(payload, length, &checksum); + if (payload != nullptr) + calc_checksum(payload, length, &checksum); // Send message write(_fd, (const void *)&header, sizeof(header)); - write(_fd, (const void *)payload, length); + if (payload != nullptr) + write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 8fd8d4da6..c8c89dfad 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -72,6 +72,7 @@ #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 */ @@ -87,6 +88,7 @@ #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) /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ @@ -249,6 +251,17 @@ typedef struct { uint32_t pullL; } ubx_payload_rx_mon_hw_ubx7_t; +/* Rx MON-VER Part 1 */ +typedef struct { + 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 extension[30]; +} ubx_payload_rx_mon_ver_part2_t; + /* Rx ACK-ACK */ typedef union { uint16_t msg; @@ -333,6 +346,8 @@ typedef union { 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; @@ -366,7 +381,6 @@ typedef enum { UBX_RXMSG_ERROR_LENGTH } ubx_rxmsg_state_t; - /* ACK state */ typedef enum { UBX_ACK_IDLE = 0, @@ -400,7 +414,8 @@ private: * Add payload rx byte */ int payload_rx_add(const uint8_t b); - int payload_rx_add_svinfo(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 @@ -455,6 +470,8 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; + char _ubx_sw_version[30]; + char _ubx_hw_version[10]; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 0ffb123701c2e6124d7c735cdb55c250564d12c6 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 16 Jun 2014 15:35:38 +0200 Subject: Store hash instead of full strings for SW & HW version --- src/drivers/gps/ubx.cpp | 48 +++++++++++++++++++++++++++++++++++++++--------- src/drivers/gps/ubx.h | 8 ++++++-- 2 files changed, 45 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index be19ce488..adc7130dd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -72,6 +72,10 @@ #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 + + /**** 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() */ @@ -90,11 +94,10 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate _got_posllh(false), _got_velned(false), _disable_cmd_last(0), - _ack_waiting_msg(0) + _ack_waiting_msg(0), + _ubx_version(0) { decode_init(); - memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version)); - memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version)); } UBX::~UBX() @@ -633,18 +636,19 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) _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 - strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version)); - strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version)); - UBX_WARN("VER sw %30s", _ubx_sw_version); - UBX_WARN("VER hw %10s", _ubx_hw_version); + // 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); + UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); } } @@ -877,3 +881,29 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len write(_fd, (const void *)payload, length); write(_fd, (const void *)&checksum, sizeof(checksum)); } + +uint32_t +UBX::fnv1_32_str(uint8_t *str, uint32_t hval) +{ + 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 + + /* xor the bottom with the current octet */ + hval ^= (uint32_t)*s++; + } + + /* return our new hash value */ + return hval; +} + diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index c8c89dfad..f01b697a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -452,6 +452,11 @@ private: */ int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report); + /** + * 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; @@ -470,8 +475,7 @@ private: hrt_abstime _disable_cmd_last; uint16_t _ack_waiting_msg; ubx_buf_t _buf; - char _ubx_sw_version[30]; - char _ubx_hw_version[10]; + uint32_t _ubx_version; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From f1a5be1f57d9a15f29a428e962c3ed7f75777d22 Mon Sep 17 00:00:00 2001 From: Kynos Date: Sat, 21 Jun 2014 20:42:33 +0200 Subject: Define NAV-PVT message --- src/drivers/gps/ubx.h | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) (limited to 'src') diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index f01b697a3..1a9e7cb0c 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -63,6 +63,7 @@ /* 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 @@ -78,6 +79,7 @@ /* 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) @@ -90,6 +92,18 @@ #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) */ + /* TX CFG-PRT message contents */ #define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */ #define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ @@ -162,6 +176,42 @@ typedef struct { uint32_t reserved2; } ubx_payload_rx_nav_sol_t; +/* Rx NAV-PVT */ +typedef struct { + 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; /**< Heading of vehicle (2-D) [1e-5 deg] */ + uint32_t reserved4; +} ubx_payload_rx_nav_pvt_t; + /* Rx NAV-TIMEUTC */ typedef struct { uint32_t iTOW; /**< GPS Time of Week [ms] */ -- cgit v1.2.3 From 179bace35aaa32a23ebd7a9cb99d54eae53690f5 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 07:47:16 -0400 Subject: Resets XY velocities when we can't estimate them --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 +++++++ .../position_estimator_inav/position_estimator_inav_params.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 3 files changed, 12 insertions(+) (limited to 'src') 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 f908d7a3b..5f132453b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -916,6 +916,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_reset_v); + inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); } /* detect land */ @@ -931,6 +935,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) { 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..b79e00274 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_RESET_V, 0.2f); 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_reset_v = param_find("INAV_W_XY_RESET_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_reset_v, &(p->w_xy_reset_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..9be13637d 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_reset_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_reset_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 571e139c24464902470901501c5bbfb618c1258c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 21:33:03 +0200 Subject: commander: Formatting and logic flow cleanup, no functional change --- src/modules/commander/state_machine_helper.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..1a5a52a02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * 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 @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -133,15 +134,20 @@ 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 // 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 we need safety switch press + if (safety->safety_switch_available && !safety->safety_off) { - valid_transition = false; + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From b1f223b468ab5ff73c6a39749dbaf43f2f46a90b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 22:22:56 +0200 Subject: commander: Default all leds to off --- src/modules/commander/commander_helper.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') 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); -- cgit v1.2.3 From 211c721b48c5856ace7f09e88706d799299ca6bb Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:42:25 -0400 Subject: Shorten the reset param name to INAV_W_XY_RES_V --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- .../position_estimator_inav/position_estimator_inav_params.c | 6 +++--- .../position_estimator_inav/position_estimator_inav_params.h | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src') 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 5f132453b..eca406acf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -918,8 +918,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } else { /* gradually reset xy velocity estimates */ - inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_reset_v); - inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_reset_v); + 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 */ 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 b79e00274..47c1a4c6c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -46,7 +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_RESET_V, 0.2f); +PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.2f); 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); @@ -66,7 +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_reset_v = param_find("INAV_W_XY_RESET_V"); + 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"); @@ -89,7 +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_reset_v, &(p->w_xy_reset_v)); + 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 9be13637d..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,7 +47,7 @@ struct position_estimator_inav_params { float w_xy_gps_p; float w_xy_gps_v; float w_xy_flow; - float w_xy_reset_v; + float w_xy_res_v; float w_gps_flow; float w_acc_bias; float flow_k; @@ -67,7 +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_reset_v; + param_t w_xy_res_v; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; -- cgit v1.2.3 From 7a45888e78b4af0c04014c20e579797b2040edf8 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 24 Jun 2014 17:56:36 -0400 Subject: Adjusted default reset param value to reset quicker --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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 47c1a4c6c..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,7 +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.2f); +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); -- cgit v1.2.3 From 1cd82fc89c70060947683851af28556bccd675a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From 435cacb0a0c42b9995edc57cc33e68ff324e6106 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..abb917873 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From a1132580c55512a56ec7646f757099409ebad781 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:11:39 +0200 Subject: commander: Add pre-arm check for main sensors. --- src/modules/commander/state_machine_helper.cpp | 135 +++++++++++++++++++++---- 1 file changed, 118 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1a5a52a02..6816d46ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,14 +46,18 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include +#include +#include #include #include @@ -66,6 +70,8 @@ #endif static const int ERROR = -1; +static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; @@ -108,18 +114,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; @@ -139,12 +158,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current 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 - if (safety->safety_switch_available && !safety->safety_off) { + } else if (safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -173,17 +194,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current status->arming_state = new_arming_state; arming_state_changed = true; } - } - /* 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]); } @@ -496,6 +515,88 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f return ret; } +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + warnx("PREARM"); + + 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) { + int 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; + } + } + + warnx("PRE RES: %d", ret); + +system_eval: + close(fd); + return ret; +} + // /* -- cgit v1.2.3 From 690edbd6723746699067f69fd3c76d4d3806d9c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:14:49 +0200 Subject: Remove duplicate code in the arming check --- src/modules/commander/commander.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 588f48225..8e9352394 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1232,12 +1232,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_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } -- cgit v1.2.3 From b20d0c0f015fb62955f7a399b50626a0d57c5f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:36:43 +0200 Subject: commander: Remove prearm test print statements --- src/modules/commander/state_machine_helper.cpp | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6816d46ab..8ce724b4c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -517,8 +517,6 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - warnx("PREARM"); - int ret; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -590,8 +588,6 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } } - warnx("PRE RES: %d", ret); - system_eval: close(fd); return ret; -- cgit v1.2.3 From 263a2fa9b2258c02bdb0df6746bc289338d63cca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Jun 2014 14:52:09 +0200 Subject: mtecs: add flightpathangle filter --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 19 +++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 34 +++++++++++++--------- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 7 +++++ src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 +- src/modules/uORB/topics/tecs_status.h | 1 + 6 files changed, 43 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 48c9079a4..fc0a2551c 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _flightPathAngleLowpass(this, "FPA_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); - /* Filter arispeed */ + /* Filter airspeed */ float airspeedFiltered = _airspeedLowpass.update(airspeed); /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ @@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng } /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; _status.airspeedFiltered = airspeedFiltered; @@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* update parameters first */ updateParams(); + /* Filter flightpathangle */ + float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); + /* calculate values (energies) */ - float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); @@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; - float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; - float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; @@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; } else if (mode == TECS_MODE_LAND) { // only limit pitch but do not limit throttle @@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.flightPathAngleFiltered = flightPathAngleFiltered; _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; _status.totalEnergyRateSp = totalEnergyRateSp; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index c102f5dee..efa89a5d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,12 +104,17 @@ protected: uORB::Publication _status; /**< publish internal values for logging */ /* control blocks */ - BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ - BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output + is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: + output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight + path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration + setpoint */ /* Other calculation Blocks */ + control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ @@ -118,21 +123,22 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ - BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in + last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ - hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; - bool _debug; ///< Set true to enable debug output + bool _debug; ///< Set true to enable debug output static void debug_print(const char *fmt, va_list args); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index bbbb8f9db..5b9238780 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); +/** + * Lowpass (cutoff freq.) for the flight path angle + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); + /** * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0813bf7b0..ab9c1639c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,6 +1507,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5645ebcf1..a631317df 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -353,6 +353,7 @@ struct log_TECS_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; @@ -431,7 +432,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 05310e906..c4d0c1874 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -68,6 +68,7 @@ struct tecs_status_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; -- cgit v1.2.3 From d5b5dcef24a7f5c7b2ed98081dd69a05f8d46959 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:01:43 -0700 Subject: Fix bugs found through compiler warnings --- src/drivers/mkblctrl/mkblctrl.cpp | 8 ++++---- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 10 ++++++++-- src/modules/commander/commander.cpp | 1 + src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- src/modules/gpio_led/gpio_led.c | 3 ++- src/modules/px4iofirmware/sbus.c | 10 ++++++++-- src/systemcmds/mtd/mtd.c | 8 ++++++-- src/systemcmds/tests/test_mixer.cpp | 1 + 10 files changed, 33 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..493678ae5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..b0a8527c2 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1535967b1..3cd250458 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..f8b9e9fbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt posNED[2] = -(hgt - hgtRef); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..69d8cfce8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..1fc6d1295 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - warnx("stop"); + // FIXME: Is this correct? changed from warnx + errx(0, "stop"); } else { errx(1, "not running"); diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..8ed69678c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,19 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..8b43e54da 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..dc2c20e79 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int -- cgit v1.2.3 From f0558a97ddbbd275022766603a866e86316bbb5b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:53:59 -0700 Subject: Use same warnx/exit pattern --- src/modules/gpio_led/gpio_led.c | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1fc6d1295..def958c95 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,17 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); + exit(0); } - - exit(0); - - } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - // FIXME: Is this correct? changed from warnx - errx(0, "stop"); - + warnx("stop"); + exit(0); } else { errx(1, "not running"); } -- cgit v1.2.3 From 367613a1b57f2c52e97958ab40426fc77245383a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:32:58 -0700 Subject: Fix tabbing --- src/drivers/mkblctrl/mkblctrl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 493678ae5..1c81910f6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - void set_px4mode(int px4mode); - void set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: -- cgit v1.2.3 From 33c20e15a2246e9249bbb8183758604fdc231183 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:35:15 -0700 Subject: Backing out sbus changes --- src/modules/px4iofirmware/sbus.c | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 8ed69678c..0e7dc621c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,19 +119,13 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - // FIXME: I assume this was previously NYI? - ssize_t cBytes = num_values * sizeof(values[0]); - ssize_t cBytesWritten = write(sbus_fd, values, cBytes); - return cBytesWritten == cBytes; + write(sbus_fd, 'A', 1); } bool sbus2_output(uint16_t *values, uint16_t num_values) { - // FIXME: I assume this was previously NYI? - ssize_t cBytes = num_values * sizeof(values[0]); - ssize_t cBytesWritten = write(sbus_fd, values, cBytes); - return cBytesWritten == cBytes; + write(sbus_fd, 'B', 1); } bool -- cgit v1.2.3 From f3af2f9f624bb3111b90d8b6d428d99de8ad3a8e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:47:56 -0700 Subject: Fix tabbing to 8 spaces, keep tab --- src/modules/mavlink/mavlink_ftp.cpp | 84 ++++++++++++++++++------------------- src/modules/mavlink/mavlink_ftp.h | 4 +- 2 files changed, 44 insertions(+), 44 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f1436efb8..ca846a465 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -127,7 +127,7 @@ MavlinkFTP::_worker(Request *req) break; case kCmdTerminate: - errorCode = _workTerminate(req); + errorCode = _workTerminate(req); break; case kCmdReset: @@ -222,12 +222,12 @@ MavlinkFTP::_workList(Request *req) // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { - // User is requesting subsequent dir entries but there were none. This means the user asked - // to seek past EOF. - errorCode = kErrEOF; - } - // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that break; } @@ -266,18 +266,18 @@ MavlinkFTP::_workOpen(Request *req, bool create) { auto hdr = req->header(); - int session_index = _findUnusedSession(); + int session_index = _findUnusedSession(); if (session_index < 0) { return kErrNoSession; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(req->dataAsCString(), oflag); if (fd < 0) { return create ? kErrPerm : kErrNotFile; } - _session_fds[session_index] = fd; + _session_fds[session_index] = fd; hdr->session = session_index; hdr->size = 0; @@ -290,28 +290,28 @@ MavlinkFTP::_workRead(Request *req) { auto hdr = req->header(); - int session_index = hdr->session; - - if (!_validSession(session_index)) { + int session_index = hdr->session; + + if (!_validSession(session_index)) { return kErrNoSession; - } - - // Seek to the specified position - printf("Seek %d\n", hdr->offset); + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { - // Unable to see to the specified location + // Unable to see to the specified location return kErrEOF; } - + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); if (bytes_read < 0) { - // Negative return indicates error other than eof + // Negative return indicates error other than eof return kErrIO; } - - printf("Read success %d\n", bytes_read); + + printf("Read success %d\n", bytes_read); hdr->size = bytes_read; - + return kErrNone; } @@ -357,26 +357,26 @@ MavlinkFTP::_workTerminate(Request *req) { auto hdr = req->header(); - if (!_validSession(hdr->session)) { + if (!_validSession(hdr->session)) { return kErrNoSession; - } + } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[hdr->session]); - return kErrNone; + return kErrNone; } MavlinkFTP::ErrorCode MavlinkFTP::_workReset(void) { - for (size_t i=0; i Date: Sun, 29 Jun 2014 13:55:55 -0700 Subject: More tab fixes --- src/drivers/roboclaw/RoboClaw.cpp | 8 ++++---- src/modules/commander/commander.cpp | 2 +- src/modules/gpio_led/gpio_led.c | 4 ++-- src/systemcmds/mtd/mtd.c | 12 ++++++------ src/systemcmds/tests/test_mixer.cpp | 2 +- 5 files changed, 14 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b0a8527c2..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -183,8 +183,8 @@ float RoboClaw::getMotorPosition(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Position; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } @@ -195,8 +195,8 @@ float RoboClaw::getMotorSpeed(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Speed; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3cd250458..aa243d79b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,7 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } - return true; + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index def958c95..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,13 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); - exit(0); + exit(0); } } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - exit(0); + exit(0); } else { errx(1, "not running"); } diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8b43e54da..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,12 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) { - // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried - // that but setting the bug speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to warnx. - warnx("failed to set bus speed"); - } + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc2c20e79..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,7 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); - return 0; + return 0; } static int -- cgit v1.2.3 From 43b9d96cf4306bc472d16956f5437dbef7c630c0 Mon Sep 17 00:00:00 2001 From: Kynos Date: Mon, 30 Jun 2014 01:55:40 +0200 Subject: Merged upstream master, add missing change --- src/modules/mavlink/mavlink_messages.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 472e2810d..06c2e737e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -669,16 +669,16 @@ protected: if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph), - 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_used); + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph), + 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_used); } } }; -- cgit v1.2.3 From 92adbe9216c96c53d1baa4eb1e14b4ede272c080 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 17:47:24 -0700 Subject: Fix compiler warnings --- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 4 +-- src/drivers/gps/gps.cpp | 6 ++-- src/drivers/hmc5883/hmc5883.cpp | 4 +-- src/drivers/mkblctrl/mkblctrl.cpp | 19 ++++------ src/drivers/px4fmu/fmu.cpp | 12 +++---- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/examples/px4_simple_app/px4_simple_app.c | 1 + src/lib/geo/geo.c | 33 +++++++++-------- src/lib/geo_lookup/geo_mag_declination.c | 31 +++++++--------- .../attitude_estimator_ekf_params.c | 2 +- .../attitude_estimator_so3_main.cpp | 21 +++-------- .../commander/accelerometer_calibration.cpp | 6 ++-- src/modules/commander/calibration_routines.cpp | 2 +- src/modules/dataman/dataman.c | 3 +- .../ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/fw_pos_control_l1/landingslope.cpp | 16 ++++----- src/modules/fw_pos_control_l1/landingslope.h | 8 ++--- src/modules/mavlink/mavlink_main.cpp | 6 ++-- src/modules/mavlink/mavlink_main.h | 4 +-- src/modules/mavlink/mavlink_orb_subscription.cpp | 6 ++-- src/modules/mavlink/mavlink_stream.cpp | 6 +++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 +- .../position_estimator_inav_main.c | 41 ++++++++++------------ src/modules/px4iofirmware/protocol.h | 2 +- src/modules/px4iofirmware/px4io.c | 5 ++- src/modules/px4iofirmware/registers.c | 3 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/hx_stream.c | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 4 +-- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/param/param.c | 2 -- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 - src/systemcmds/mtd/mtd.c | 30 ++++++++-------- src/systemcmds/param/param.c | 4 +-- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 7 ++-- src/systemcmds/tests/test_file2.c | 10 +++--- src/systemcmds/tests/test_float.c | 10 +++--- src/systemcmds/tests/test_mathlib.cpp | 4 --- src/systemcmds/tests/test_mixer.cpp | 33 ++++++----------- src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 1 + src/systemcmds/tests/test_servo.c | 1 + 45 files changed, 162 insertions(+), 206 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index a856ccb02..3c05bfa46 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -54,7 +54,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); extern void led_toggle(int led); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 2de7063ea..1a7e068fe 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -364,7 +364,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -389,7 +389,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 8560716e9..dd505abdb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -357,7 +357,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -449,7 +449,7 @@ GPS::print_info() 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.0f); + _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); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); @@ -578,7 +578,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e..b7b368a5e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), + _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -1228,7 +1228,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)1.0/_range_scale, (double)_range_ga); + (double)(1.0f/_range_scale), (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1c81910f6..3996b76a6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) : _task(-1), _t_actuators(-1), _t_actuator_armed(-1), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), _t_outputs(0), _t_esc_status(0), _num_outputs(0), + _primary_pwm_device(false), _motortest(false), _overrideSecurityChecks(false), - _motor(-1), - _px4mode(MAPPING_MK), - _frametype(FRAME_PLUS), - _primary_pwm_device(false), _task_should_exit(false), _armed(false), _mixers(nullptr) @@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) void MK::task_main() { - long update_rate_in_us = 0; - float tmpVal = 0; - /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -483,7 +480,6 @@ MK::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - update_rate_in_us = long(1000000 / _update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { @@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val) _retries = 0; uint8_t result[3] = { 0, 0, 0 }; uint8_t msg[2] = { 0, 0 }; - uint8_t rod = 0; uint8_t bytesToSendBL2 = 2; tmpVal = val; @@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val) if (debugCounter == 2000) { debugCounter = 0; - for (int i = 0; i < _num_outputs; i++) { + for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } @@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned motors, char *device_path) +mk_start(unsigned motors, const char *device_path) { int ret; @@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; - char *devicepath = ""; + const char *devicepath = ""; /* * optional parameters diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 84ea9a3bc..0a4635728 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -240,8 +240,6 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _control_subs({-1}), - _poll_fds_num(0), _armed_sub(-1), _outputs_pub(-1), _num_outputs(0), @@ -252,10 +250,12 @@ PX4FMU::PX4FMU() : _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _control_subs{-1}, + _poll_fds_num(0), + _failsafe_pwm{0}, + _disarmed_pwm{0}, + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 3b210ac59..c39494fb0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9109af14f..bca1715fa 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -616,7 +616,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 44e6dc7f3..4e9f099ed 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..212e33ff8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ @@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; - *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; - *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel @@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); @@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } if (arc_sweep >= 0) { @@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // as this function generally will not be called repeatedly when we are out of the sector. // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float start_disp_x = radius * sinf(arc_start_bearing); + float start_disp_y = radius * cosf(arc_start_bearing); + float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0f; + float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = lon_now + end_disp_x / 111111.0f; + float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now double d_lat = x_rad - current_x_rad; double d_lon = y_rad - current_y_rad; - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); double c = 2 * atan2(sqrt(a), sqrt(1 - a)); float dxy = CONSTANTS_RADIUS_OF_EARTH * c; diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index 09eac38f4..c41d52606 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -54,24 +54,19 @@ static const int8_t declination_table[13][37] = \ { - 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ - -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ - -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ - 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ - -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ - 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ - 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ - -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ - -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ - 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ - 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ - 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ - 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ - 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ - -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ - 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ - 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ - 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 49a892609..bc0e3b93a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI / 180.0f; + p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e55b01160..e49027e47 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl */ int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - //! Time constant float dt = 0.005f; @@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ @@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { @@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { - update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { - update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { - update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_start = hrt_absolute_time(); - // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], @@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7180048ff..24da452b1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { + int fd; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { @@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd) int res = OK; /* reset all offsets to zero and all scales to one */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd) if (res == OK) { /* apply new scaling and offsets */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index be38ea104..9d79124e7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], float aA, aB, aC, nA, nB, nC, dA, dB, dC; //Iterate N times, ignore stop condition. - int n = 0; + unsigned int n = 0; while (n < max_iterations) { n++; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..406293bda 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -50,6 +50,7 @@ #include #include "dataman.h" +#include /** * data manager app start / stop handling function @@ -187,7 +188,7 @@ create_work_item(void) if (item) { item->first = 1; lock_queue(&g_free_q); - for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 7aad849f9..15ceb57c0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -234,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); +void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 8ce465fe8..42e00da05 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,16 +46,16 @@ #include #include -void Landingslope::update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt) +void Landingslope::update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new) { - _landing_slope_angle_rad = landing_slope_angle_rad; - _flare_relative_alt = flare_relative_alt; - _motor_lim_relative_alt = motor_lim_relative_alt; - _H1_virt = H1_virt; + _landing_slope_angle_rad = landing_slope_angle_rad_new; + _flare_relative_alt = flare_relative_alt_new; + _motor_lim_relative_alt = motor_lim_relative_alt_new; + _H1_virt = H1_virt_new; calculateSlopeValues(); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index b54fd501c..a5975ad43 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -123,10 +123,10 @@ public: float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); - void update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt); + void update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a5e31ef4..43acee96f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; @@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6777d56c3..5f4117ae5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,9 +123,9 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); static int instance_count(); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,10 +47,10 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { } diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5ec30bd33..ed6776e5c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,7 +43,11 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : + _last_sent(0), + _channel(MAVLINK_COMM_0), + _interval(1000000), + next(nullptr) { } 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 09960d106..c24c172af 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -545,7 +545,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -862,7 +861,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ - if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); 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 62cee145e..9870ebd05 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -179,15 +179,21 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", + hrt_absolute_time(), msg, (double)dt, + (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], + (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + (double)acc[0], (double)acc[1], (double)acc[2], + (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], + (double)w_xy_gps_p, (double)w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -261,9 +267,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame @@ -285,7 +288,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) - hrt_abstime xy_src_time = 0; // time of last available position data bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid @@ -370,8 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); + warnx("baro offs: %.2f", (double)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -475,7 +477,7 @@ 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.7 && 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 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -497,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); } } else { @@ -510,7 +512,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ @@ -558,7 +560,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1 / w_flow; + eph_flow = 0.1f / w_flow; flow_valid = true; @@ -661,8 +663,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + 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); } } @@ -746,10 +748,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { - eph *= 1.0 + dt; + eph *= 1.0f + dt; } if (epv < max_eph_epv) { - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ @@ -758,11 +760,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - /* try to estimate position during some time after position sources lost */ - if (use_gps_xy || use_flow) { - xy_src_time = t; - } - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 7471faec7..d5a6b1ec4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -312,7 +312,7 @@ struct IOPacket { #define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) static const uint8_t crc8_tab[256] __attribute__((unused)) = { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d4c25911e..cd134ccb4 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -37,6 +37,7 @@ */ #include +#include #include // required for task_create #include @@ -303,14 +304,12 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)minfo.mxordblk); + (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index db1836f4a..50108ea1b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] = [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; @@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16fcb4c26..8908adf4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -649,7 +649,7 @@ Sensors::parameters_update() if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8e9c2bfcf..52ae77de5 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -63,7 +63,7 @@ struct hx_stream { /* TX state */ int fd; bool tx_error; - uint8_t *tx_buf; + const uint8_t *tx_buf; unsigned tx_resid; uint32_t tx_crc; enum { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 092c0e2b0..4b22a46d0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; - const char *end = buf + buflen; /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { @@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float scale_yaw = 1.0f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) } /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0) { + if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 695574fdc..0548a9f7d 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -133,7 +133,7 @@ int lock_otp(void) // COMPLETE, BUSY, or other flash error? -int F_GetStatus(void) +static int F_GetStatus(void) { int fs = F_COMPLETE; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 7a499ca72..e44e6cdb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_sem = { .semcount = 1 }; - /** lock the parameter store */ static void param_lock(void) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index ea5ba9e52..c733a53d7 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } unsigned progress; - uint16_t temp_pwm; /* then set effective_pwm based on state */ switch (limit->state) { diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index fcc9b8366..a925cdd40 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -91,7 +91,7 @@ static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); -static void mtd_print_info(); +static void mtd_print_info(void); static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); @@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0; static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); +static void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + int mtd_main(int argc, char *argv[]) { if (argc >= 2) { @@ -355,7 +365,7 @@ static ssize_t mtd_get_partition_size(void) return partsize; } -void mtd_print_info() +void mtd_print_info(void) { if (!attached) exit(1); @@ -385,16 +395,6 @@ mtd_test(void) exit(1); } -void -mtd_status(void) -{ - if (!attached) - errx(1, "MTD driver not started"); - - mtd_print_info(); - exit(0); -} - void mtd_erase(char *partition_names[], unsigned n_partitions) { @@ -428,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) uint8_t v[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; + ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); if (fd == -1) { @@ -459,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) uint8_t v[128], v2[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; - off_t offset = 0; + ssize_t count = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); if (fd == -1) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index d92ee88ef..328205e29 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_show(const char* search_string); 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(); +static void do_reset(void); int param_main(int argc, char *argv[]) @@ -416,7 +416,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons) } static void -do_reset() +do_reset(void) { param_reset_all(); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 50dece816..0ecdc8eb9 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)), fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", 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 96be1e8df..86f23b485 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -102,7 +102,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { @@ -116,7 +116,7 @@ test_file(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -224,9 +224,6 @@ test_file(int argc, char *argv[]) return 1; } - /* compare value */ - bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index ef555f6c3..8db3ea5ae 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -49,6 +49,8 @@ #include #include +#include "tests.h" + #define FLAG_FSYNC 1 #define FLAG_LSEEK 2 @@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t buffer[j] = get_value(ofs); ofs++; } - if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { - printf("write failed at offset %u\n", ofs); - exit(1); + if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { + printf("write failed at offset %u\n", ofs); + exit(1); } if (flags & FLAG_FSYNC) { fsync(fd); @@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t printf("read ofs=%u\r", ofs); } counter++; - if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); exit(1); } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 4921c9bbb..e33cc6ef7 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -94,7 +94,7 @@ int test_float(int argc, char *argv[]) printf("\t success: asinf(1.0f) == 1.57079f\n"); } else { - printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one); ret = -1; } @@ -128,7 +128,7 @@ int test_float(int argc, char *argv[]) float sinf_zero_one = sinf(0.1f); - if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { printf("\t success: sinf(0.1f) == 0.09983f\n"); } else { @@ -155,7 +155,7 @@ int test_float(int argc, char *argv[]) } char sbuf[30]; - sprintf(sbuf, "%8.4f", 0.553415f); + sprintf(sbuf, "%8.4f", (double)0.553415f); if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -166,7 +166,7 @@ int test_float(int argc, char *argv[]) ret = -5; } - sprintf(sbuf, "%8.4f", -0.553415f); + sprintf(sbuf, "%8.4f", (double)-0.553415f); if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -205,7 +205,7 @@ int test_float(int argc, char *argv[]) printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { - printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1); + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1); ret = -8; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 693a208ba..00c649c75 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -52,10 +52,6 @@ using namespace math; -const char* formatResult(bool res) { - return res ? "OK" : "ERROR"; -} - int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 0b826b826..8ab8fa2d6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[]) warnx("testing mixer"); - char *filename = "/etc/mixers/IO_pass.mix"; + const char *filename = "/etc/mixers/IO_pass.mix"; if (argc > 1) filename = argv[1]; @@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[]) * e.g. on PX4IO. */ - unsigned nused = 0; - const unsigned chunk_size = 64; MixerGroup mixer_group(mixer_callback, 0); @@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[]) return 1; /* FIRST mark the mixer as invalid */ - bool mixer_ok = false; /* THEN actually delete it */ mixer_group.reset(); char mixer_text[256]; /* large enough for one mixer */ @@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[]) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - bool mixer_ok = false; return 1; } @@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - mixer_ok = true; - } else { - /* not yet reached the end of the mixer, set as not ok */ - mixer_ok = false; - } - warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[]) should_arm = true; /* run through arming phase */ - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && @@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[]) return 1; } - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); sleepcount++; @@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = j/10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } @@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; @@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index b9041b013..addd57bea 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[]) int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 57c0e7f4c..c9f9ef439 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include "tests.h" diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ef8512bf5..9c6951ca2 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -51,6 +51,7 @@ #include #include +#include #include -- cgit v1.2.3 From b9e2fa2c0dc77e41846b6982f057cbbc9e60be89 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:03:48 +0200 Subject: mc_pos_control: compiler warning fix --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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 */ -- cgit v1.2.3 From 8e41cbfdfa2083c421954b6d9aadde5c0e508cdc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:18:38 +0200 Subject: mavlink: Warning fixes --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_main.h | 12 ++++++------ src/modules/mavlink/mavlink_stream.cpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) (limited to 'src') 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_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) { } -- cgit v1.2.3 From 3f6851b9879c2e4d764926a3bc29ff800a17b73d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:08 +0200 Subject: Re-send the RC config warnings once the GCS is connected, fixed compile warnings --- .../commander/accelerometer_calibration.cpp | 3 ++- src/modules/commander/calibration_routines.cpp | 7 +++--- src/modules/commander/commander.cpp | 26 +++++++++++++++++----- 3 files changed, 27 insertions(+), 9 deletions(-) (limited to 'src') 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 #include #include +#include #include #include #include @@ -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 +#include #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..ef4c18ea6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,16 +371,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 +507,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; @@ -752,7 +759,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; @@ -862,6 +868,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) { @@ -915,6 +922,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) { -- cgit v1.2.3 From ee8f4dcee639effe982bbb099a6f86f7c4fce5e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:44 +0200 Subject: systemcmds: Warning fixes --- src/systemcmds/mtd/24xxxx_mtd.c | 4 +++- src/systemcmds/pwm/pwm.c | 3 ++- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 3 ++- src/systemcmds/tests/test_float.c | 14 +++++++------- src/systemcmds/tests/test_mtd.c | 2 ++ 6 files changed, 17 insertions(+), 11 deletions(-) (limited to 'src') 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/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); -- cgit v1.2.3 From 35cd487a428a9c76609faa8ae0f99afdc376b5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:02 +0200 Subject: drivers: Warning fixes --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src') 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 */ -- cgit v1.2.3 From 20de2da032083187bd89914942ffc8574904c885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:23 +0200 Subject: Navigator: Warning fixes --- src/modules/navigator/mission_block.cpp | 4 +++- src/modules/navigator/rtl.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c7..26a573544 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include #include @@ -222,7 +224,7 @@ MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() + || (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON) || pos_sp_triplet->current.loiter_direction != 1 || pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 043f773a4..597a2c0ec 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -227,7 +227,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) _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"); -- cgit v1.2.3 From a66f88b29a93be4c3f66392994d55b4dc03d428e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:20:47 +0200 Subject: systemlib: Warning fixes --- src/modules/systemlib/rc_check.c | 1 + src/modules/systemlib/systemlib.c | 3 +++ 2 files changed, 4 insertions(+) (limited to 'src') diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index c0c1a5cb4..b2b2c1290 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -42,6 +42,7 @@ #include #include +#include #include #include #include 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); -- cgit v1.2.3 From 751f8462f6aa298a59619484a13adedfdd521458 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:21:23 +0200 Subject: IO firmware: Warning / bug fixes --- src/modules/px4iofirmware/i2c.c | 2 ++ src/modules/px4iofirmware/sbus.c | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) (limited to 'src') 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 -- cgit v1.2.3 From 083d605f8a9b00a1551572008e487279c39f16aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:22:06 +0200 Subject: sensors: Warning fixes --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8908adf4c..8cfe35c42 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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 -- cgit v1.2.3 From b5f9e95af0be6208a1eb411ed56c830d762227f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:22:26 +0200 Subject: INAV: Warning fixes --- .../position_estimator_inav/position_estimator_inav_main.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) (limited to 'src') 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..488b53453 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 #include #include +#include #include #include #include @@ -477,7 +478,7 @@ 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]; @@ -952,11 +953,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; -- cgit v1.2.3 From 04efee0bc8a2b0e8e9ffae4cf2c5b28bb1ba9ba9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:23:38 +0200 Subject: nshterm: Hotfix for retries counter --- src/systemcmds/nshterm/nshterm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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 \n"); exit(1); } - uint8_t retries = 0; + unsigned retries = 0; int fd = -1; /* try the first 30 seconds */ -- cgit v1.2.3 From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e24..a15712a91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d4..7a61d481b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // 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.", status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From ad4411bfc1894c8a9ca42f563f7bf8207b39e7c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:32:32 +0200 Subject: Fix line endings for patch --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') 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 488b53453..5b312941e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -478,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 && (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + 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]; -- cgit v1.2.3 From bbdf2bc07aaaa363ef58f15831511c64c2d856bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:58:05 +0200 Subject: commander: Notify negative on manual arming fails --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e4a709ef6..efa26eb97 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1342,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 */ -- cgit v1.2.3 From 8408993a676acd0149e880ffeded74ce9c67b9c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 17:45:50 +0200 Subject: Hotfix, non-code change: Raise RC config error warnings to audio --- src/modules/systemlib/rc_check.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b2b2c1290..b35b333af 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -99,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++; @@ -140,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); } -- cgit v1.2.3 From 45433c3711dba5c972e02e8c9263853164670a86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:18 +0200 Subject: Hotfix: Fix use of circuit breaker param --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b59531d69..8f697749e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -86,7 +86,7 @@ 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); + (void)param_get(param_find(breaker), &val); return (val == magic); } -- cgit v1.2.3 From fb98251e4d08d351c7ba404e3816958435c57515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:36 +0200 Subject: Warning fixes in commander --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current 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.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } @@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); -- cgit v1.2.3 From 534c377ec6d09aff0feed9eb7e70c9188abcda8c Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 1 Jul 2014 10:49:59 +0200 Subject: TECS: Small fix to throttle feed-forward --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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; -- cgit v1.2.3 From 6042999aab26276ff87e7274b97b69200919b86f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 11:59:25 +0200 Subject: Add vital comment to circuit breaker --- src/modules/systemlib/circuit_breaker.h | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src') diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 86439e2a5..1175dbce8 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -40,6 +40,15 @@ #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 -- cgit v1.2.3 From d0b78aa8a09cd4259258ab10b77c98e907c3b460 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 13:42:32 +0200 Subject: Add param command which does not reset the autostart params to not hamper auto-config --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- src/systemcmds/param/param.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 30 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5856ba84f..1c9ded6a8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -134,8 +134,8 @@ then then # We can't be sure the defaults haven't changed, so # if someone requests a re-configuration, we do it - # cleanly from scratch - param reset + # cleanly from scratch (except autostart / autoconfig) + param reset_nostart set DO_AUTOCONFIG yes else set DO_AUTOCONFIG no 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); + } +} -- cgit v1.2.3 From 4cd66a3242aa2dfb03a3e58ffcc8d27e1350b7ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 00:25:19 +0200 Subject: Fix sdlog2 GPS time copy for log_on_start situation --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9b071ff99..f99aec34e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1070,7 +1070,7 @@ int sdlog2_thread_main(int argc, char *argv[]) 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; } } -- cgit v1.2.3 From f428ebb04f0610c31639d8fe6d121f632c1cad1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 2 Jul 2014 11:39:56 +0200 Subject: gps: Comment-only fix --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1d8ef1b99..241e3bdf3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -109,7 +109,7 @@ 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 _mode_changed; ///< flag that the GPS mode has changed -- cgit v1.2.3 From 76aa871a7125c98e79db4dde42de7863a24a762c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 00:26:33 +0200 Subject: estimator: Fixed body / world matrix initialization and reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src') 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 -- cgit v1.2.3 From 7be2e0f13605eb9bb34eb04745a0e7a3936d94e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:18:00 +0200 Subject: Hotfix: Typo in disabled code path --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 6c0e876d1..60dcc62e0 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); + lowsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3 From 04cca73baac0b027451a76d50b1f3b365b1feeef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 3 Jul 2014 11:26:26 +0200 Subject: Hotfix of the Hotfix 8) --- src/modules/systemlib/err.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 60dcc62e0..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()); - lowsyslog(fmt, args); + lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ if (errcode < 0) -- cgit v1.2.3