diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/airspeed/airspeed.cpp | 44 | ||||
-rw-r--r-- | src/drivers/airspeed/airspeed.h | 7 | ||||
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 14 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 4 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 13 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 11 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.h | 6 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.cpp | 7 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchDetector.h | 6 | ||||
-rw-r--r-- | src/lib/launchdetection/LaunchMethod.h | 6 | ||||
-rw-r--r-- | src/lib/launchdetection/launchdetection_params.c | 5 | ||||
-rw-r--r-- | src/lib/launchdetection/module.mk | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 22 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 12 |
17 files changed, 116 insertions, 60 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 524151c90..41942aacd 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), + _last_published_sensor_ok(true), /* initialize differently to force publication */ _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _subsys_pub(-1), _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), @@ -149,8 +151,7 @@ Airspeed::init() } ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + out: return ret; } @@ -344,22 +345,6 @@ Airspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_DIFFPRESSURE - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } } void @@ -369,11 +354,34 @@ Airspeed::stop() } void +Airspeed::update_status() +{ + if (_sensor_ok != _last_published_sensor_ok) { + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _sensor_ok, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + + if (_subsys_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { + _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + + _last_published_sensor_ok = _sensor_ok; + } +} + +void Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; dev->cycle(); + dev->update_status(); } void diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 186602eda..0b8e949c9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -118,14 +118,21 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + /** + * Update the subsystem status + */ + void update_status(); + work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; + bool _last_published_sensor_ok; int _measure_ticks; bool _collect_phase; float _diff_pres_offset; orb_advert_t _airspeed_pub; + orb_advert_t _subsys_pub; int _class_instance; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d873a1132..146a06e7c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -207,14 +207,18 @@ ETSAirspeed::collect() void ETSAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -238,8 +242,12 @@ ETSAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); + } + + _sensor_ok = (ret == OK); /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cb97354ec..cdc9d3106 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -715,7 +715,7 @@ HMC5883::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -742,7 +742,7 @@ HMC5883::cycle() /* measurement phase */ if (OK != measure()) - log("measure error"); + debug("measure error"); /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1ad383ee0..c0f3c28e0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -288,13 +288,17 @@ MEASAirspeed::collect() void MEASAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -318,10 +322,13 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) { - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); } + _sensor_ok = (ret == OK); + /* next phase is collection */ _collect_phase = true; diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 12f80c4a3..c555a0a69 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -33,9 +33,10 @@ /** * @file CatapultLaunchMethod.cpp - * Catpult Launch detection + * Catapult Launch detection + * + * @author Thomas Gubler <thomasgubler@gmail.com> * - * Authors and acknowledgements in header. */ #include "CatapultLaunchMethod.h" diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 55c46ff3f..23757f6f3 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index bf539701b..3bf47bbb0 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -30,12 +30,11 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** * @file launchDetection.cpp * Auto Detection for different launch methods (e.g. catapult) * - * Authors and acknowledgements in header. + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include "LaunchDetector.h" diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 8066ebab3..1a214b66e 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 01fa7050e..e04467f6a 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -10,9 +10,9 @@ * 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 documentation4 and/or other materials provided with the + * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 45d7957f1..8df8c696c 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -18,7 +17,7 @@ * 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 + * "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, diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index 13648b74c..de0174e37 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 Estimation and Control Library (ECL). 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 diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ad435b251..e49288a74 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); +/** + * Use/Accept HIL GPS message (even if not in HIL mode) + * If set to 1 incomming HIL GPS messages are parsed. + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 227e99b48..7ecca0d65 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -208,6 +208,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _use_hil_gps(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -487,11 +488,13 @@ void Mavlink::mavlink_update_system(void) static param_t param_system_id; static param_t param_component_id; static param_t param_system_type; + static param_t param_use_hil_gps; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + param_use_hil_gps = param_find("MAV_USEHILGPS"); initialized = true; } @@ -516,6 +519,11 @@ void Mavlink::mavlink_update_system(void) if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } + + int32_t use_hil_gps; + param_get(param_use_hil_gps, &use_hil_gps); + + _use_hil_gps = (bool)use_hil_gps; } int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 66d82b471..1bf51fd31 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -157,6 +157,8 @@ public: bool get_hil_enabled() { return _hil_enabled; } + bool get_use_hil_gps() { return _use_hil_gps; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -223,6 +225,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0762b76f2..33a4fef12 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -160,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * The HIL mode is enabled by the HIL bit flag * in the system mode. Either send a set mode * COMMAND_LONG message or a SET_MODE message + * + * Accept HIL GPS messages if use_hil_gps flag is true. + * This allows to provide fake gps measurements to the system. */ if (_mavlink->get_hil_enabled()) { switch (msg->msgid) { @@ -167,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_sensor(msg); break; - case MAVLINK_MSG_ID_HIL_GPS: - handle_message_hil_gps(msg); - break; - case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: handle_message_hil_state_quaternion(msg); break; @@ -180,7 +179,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. + + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + default: + break; + } + + } + + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 41e2148ac..a73599526 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1242,7 +1242,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } _last_adc = t; - if (_battery_status.voltage_v > 0.0f) { + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); @@ -1527,12 +1527,10 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* wait for up to 50ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; + /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1571,7 +1569,7 @@ Sensors::task_main() perf_end(_loop_perf); } - printf("[sensors] exiting.\n"); + warnx("[sensors] exiting."); _sensors_task = -1; _exit(0); |