From c0a9e08a30f01026436e80d2be948acf5eb267a6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:18:05 +0200 Subject: [PATCH 01/17] ROMFS: Start GPS driver before Commander so that preflight check can be run --- ROMFS/px4fmu_common/init.d/rcS | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1237d9bb11..209994ff59 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,6 +301,18 @@ then # Sensors System (start before Commander so Preflight checks are properly run) # sh /etc/init.d/rc.sensors + + if [ $GPS == yes ] + then + if [ $GPS_FAKE == yes ] + then + echo "[i] Faking GPS" + gps start -f + else + gps start + fi + fi + unset GPS_FAKE # Needs to be this early for in-air-restarts commander start @@ -472,22 +484,10 @@ then sh /etc/init.d/rc.uavcan # - # Logging, GPS + # Logging # sh /etc/init.d/rc.logging - if [ $GPS == yes ] - then - if [ $GPS_FAKE == yes ] - then - echo "[i] Faking GPS" - gps start -f - else - gps start - fi - fi - unset GPS_FAKE - # # Start up ARDrone Motor interface # From 45f1fd663444cbba5cb7304b5ea7eda8fe0af680 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:19:30 +0200 Subject: [PATCH 02/17] Commander: Add preflight check for missing GPS module --- src/modules/commander/PreflightCheck.cpp | 26 +++++++++++++++++++++++- src/modules/commander/PreflightCheck.h | 6 +++++- src/modules/commander/commander.cpp | 9 ++++---- 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4d9bd8ae0c..893586346e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -61,6 +61,7 @@ #include #include +#include #include @@ -269,8 +270,24 @@ out: return success; } +static bool gnssCheck(int mavlink_fd) +{ + bool success = true; + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps; + + if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || + (hrt_elapsed_time(&gps.timestamp_position) > 500000)) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + success = false; + } + + close(gpsSub); + return success; +} + bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) { bool failed = false; @@ -336,6 +353,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } + /* ---- Global Navigation Satellite System receiver ---- */ + if(checkGNSS) { + if(!gnssCheck(mavlink_fd)) { + failed = true; + } + } + /* Report status */ return !failed; } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 66947a3470..b6423a4d9a 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -59,11 +59,15 @@ namespace Commander * true if the gyroscopes should be checked * @param checkBaro * true if the barometer should be checked +* @param checkAirspeed +* true if the airspeed sensor should be checked * @param checkRC * true if the Remote Controller should be checked +* @param checkGNSS +* true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7fbb0a2dac..806951cf8d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -916,6 +916,7 @@ int commander_thread_main(int argc, char *argv[]) status.circuit_breaker_engaged_airspd_check = false; status.circuit_breaker_engaged_enginefailure_check = false; status.circuit_breaker_engaged_gpsfailure_check = false; + get_circuit_breaker_params(); /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -1117,8 +1118,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_sys_type, &(status.system_type)); // get system type status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); - get_circuit_breaker_params(); - bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1127,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1300,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2749,7 +2748,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); From f020ad4ba31017925ace61fb60244f9043e4ce08 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:29:35 +0200 Subject: [PATCH 03/17] Commander: Check if GPS receiver is suffering from jamming noise --- src/modules/commander/commander.cpp | 36 ++++++++++++++++++++--------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 806951cf8d..b6deffe75c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -125,6 +125,8 @@ static const int ERROR = -1; extern struct system_load_s system_load; +static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -1634,19 +1636,31 @@ int commander_thread_main(int argc, char *argv[]) (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } - /* check if GPS fix is ok */ - if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + /* check if GPS is ok */ + if (status.circuit_breaker_engaged_gpsfailure_check) { + bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; + + //Check if GPS receiver is too noisy while we are disarmed + if (!armed.armed && gpsIsNoisy) { + if (!status.gps_failure) { + mavlink_log_critical(mavlink_fd, "GPS signal noisy"); + set_tune_override(TONE_GPS_WARNING_TUNE); + + //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight + status.gps_failure = true; + status_changed = true; + } } - } else { - if (!status.gps_failure) { + if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure && !gpsIsNoisy) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + + } else if (!status.gps_failure) { status.gps_failure = true; status_changed = true; mavlink_log_critical(mavlink_fd, "gps fix lost"); From fee02c6943101287f266eb66a8cf3733e077a0be Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 20:30:10 +0200 Subject: [PATCH 04/17] Commander: Fix parameter bug in preflight check function --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b6deffe75c..c74c620212 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1128,7 +1128,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1301,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2762,7 +2762,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 73fdb09406..990e549a05 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -689,5 +689,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status->circuit_breaker_engaged_gpsfailure_check, true); } From 52222de0213fbe196cc514296472d586ae66befe Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 20:59:23 +0200 Subject: [PATCH 05/17] Commander: Wait up to 1 second to allow GPS module to be detected --- src/modules/commander/PreflightCheck.cpp | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 893586346e..f90406cf00 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -273,14 +274,28 @@ out: static bool gnssCheck(int mavlink_fd) { bool success = true; + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || - (hrt_elapsed_time(&gps.timestamp_position) > 500000)) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + //Wait up to 1000ms to allow the driver to detect a GNSS receiver module + struct pollfd fds[1]; + fds[0].fd = gpsSub; + fds[0].events = POLLIN; + if(poll(fds, 1, 1000) <= 0) { success = false; } + else { + struct vehicle_gps_position_s gps; + if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || + (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) { + success = false; + } + } + + //Report failure to detect module + if(!success) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } close(gpsSub); return success; From 510b6124ecc7df6a10cf7ba8b87ca46e0f743e30 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 21:20:05 +0200 Subject: [PATCH 06/17] Commander: Fix inverted circuit breaker logic --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c74c620212..dae51233d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1128,7 +1128,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1301,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1637,7 +1637,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS is ok */ - if (status.circuit_breaker_engaged_gpsfailure_check) { + if (!status.circuit_breaker_engaged_gpsfailure_check) { bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; //Check if GPS receiver is too noisy while we are disarmed @@ -2762,7 +2762,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 990e549a05..cdcc12043e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -689,5 +689,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, status->circuit_breaker_engaged_gpsfailure_check, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true); } From d650820dbfafbdf17a2688143752223970b1d9d4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 21:20:34 +0200 Subject: [PATCH 07/17] SystemLib: Add missing CBRK_GPSFAIL circuit breaker parameter --- src/modules/systemlib/circuit_breaker_params.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e5cc034bc9..097903d21f 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -120,3 +120,19 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); + +/** + * Circuit breaker for GPS failure detection + * + * Setting this parameter to 240024 will disable the GPS failure detection. + * If this check is enabled, then the sensor check will fail if the GPS module + * is missing. It will also check for excessive signal noise on the GPS receiver + * and warn the user if detected. + * + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); \ No newline at end of file From 38004cdd955dad01801b750e18e45ac5dd3000e4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:31:16 +0200 Subject: [PATCH 08/17] PreflightCheck: Increase GPS timeout to 4 sec --- src/modules/commander/PreflightCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index f90406cf00..cc993bc015 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -281,7 +281,7 @@ static bool gnssCheck(int mavlink_fd) struct pollfd fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 1000) <= 0) { + if(poll(fds, 1, 4000) <= 0) { success = false; } else { From 680898e6aad91ea1de1e832c821c8fa1d3fdee0e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:48:40 +0200 Subject: [PATCH 09/17] GPS: Publish first data after configuring device --- src/drivers/gps/gps.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 714c80ded2..e3706cf671 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -354,6 +354,20 @@ GPS::task_main() if (_Helper->configure(_baudrate) == 0) { unlock(); + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); @@ -364,13 +378,7 @@ GPS::task_main() if (!(_pub_blocked)) { 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_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { From cd67609da5755dae6cd81e60bb611e498ff2d180 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:49:53 +0200 Subject: [PATCH 10/17] PreflightCheck: Reduce GPS timeout to 2 sec --- src/modules/commander/PreflightCheck.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index cc993bc015..96e7757da0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -277,11 +277,11 @@ static bool gnssCheck(int mavlink_fd) int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - //Wait up to 1000ms to allow the driver to detect a GNSS receiver module + //Wait up to 2000ms to allow the driver to detect a GNSS receiver module struct pollfd fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 4000) <= 0) { + if(poll(fds, 1, 2000) <= 0) { success = false; } else { From a90caf7b7b3e70fb61b57c6dce6710d78debbfcd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 18 May 2015 20:51:14 +0900 Subject: [PATCH 11/17] l3gd20: faster gyro interrupts --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 0319b601e7..82c18b5c14 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -195,7 +195,7 @@ static const int ERROR = -1; This time reduction is enough to cope with worst case timing jitter due to other timers */ -#define L3GD20_TIMER_REDUCTION 200 +#define L3GD20_TIMER_REDUCTION 600 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } From d9d25363b4d222db171e9ee8531b70b9a1df1e39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 07:07:54 +0200 Subject: [PATCH 12/17] mavlink FTP: Remove workaround after QGC side fix --- src/modules/mavlink/mavlink_ftp.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a6c92ac623..233bc2d323 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -381,8 +381,7 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - // XXX @DonLakeFlyer: Remove the first condition for the test setup - if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { From 66e6dccfee45a2865f79f03929bb6ae0545f5d2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 07:18:25 +0200 Subject: [PATCH 13/17] FW att control: Better param docs --- .../fw_att_control/fw_att_control_params.c | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6b248cbe2e..d9ccd8bac8 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * -f * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013-2015 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 @@ -37,8 +36,8 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Parameters defined by the fixed-wing attitude control task * - * @author Lorenz Meier - * @author Thomas Gubler + * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -73,6 +72,8 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); * This defines how much the elevator input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); @@ -144,6 +145,8 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); * This defines how much the aileron input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); @@ -190,6 +193,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); * This defines how much the rudder input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); @@ -234,7 +239,9 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); /** * Roll rate feed forward * - * Direct feed forward from rate setpoint to control surface output + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. * * @min 0.0 * @max 10.0 From 5fb99e9300c6c62c7d271c6780cbc9ea81191972 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 07:18:41 +0200 Subject: [PATCH 14/17] MC att control: Better param docs --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_params.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 ec512ab56f..34bfa46140 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -40,7 +40,7 @@ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * * @author Tobias Naegeli - * @author Lorenz Meier + * @author Lorenz Meier * @author Anton Babushkin * * The controller has two loops: P loop for angular error and PD loop for angular rate error. diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 3f19a51f06..c9bf3753b4 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 From 5c63a2d2f40b25fc4250e98917964186e40136c8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 19 May 2015 14:19:03 +0200 Subject: [PATCH 15/17] fixed sensor board rotation offset --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 153e704809..edfd5ffb82 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -856,7 +856,7 @@ Sensors::parameters_update() M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - _board_rotation = _board_rotation * board_rotation_offset; + _board_rotation = board_rotation_offset * _board_rotation; /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); From 12c6dc8ad87b1d9b6e061b7e6b801a89cd3d5cdd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 19 May 2015 14:20:00 +0200 Subject: [PATCH 16/17] added routine for autopilot level calibration --- .../commander/accelerometer_calibration.cpp | 73 +++++++++++++++++++ .../commander/accelerometer_calibration.h | 1 + src/modules/commander/commander.cpp | 7 +- 3 files changed, 80 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index efd88b3d40..f4fd88fa22 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,6 +133,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -552,3 +554,74 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref return calibrate_return_ok; } + +int do_level_calibration(int mavlink_fd) { + const unsigned cal_time = 5; + const unsigned cal_hz = 250; + const unsigned settle_time = 30; + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level"); + + param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); + param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + + float zero = 0.0f; + param_set(roll_offset_handler, &zero); + param_set(pitch_offset_handler, &zero); + + struct pollfd fds[1]; + + fds[0].fd = att_sub; + fds[0].events = POLLIN; + + float roll_mean = 0.0f; + float pitch_mean = 0.0f; + unsigned counter = 0; + + // sleep for some time + hrt_abstime start = hrt_absolute_time(); + while(hrt_elapsed_time(&start) < settle_time * 1000000) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); + sleep(settle_time / 10); + } + + start = hrt_absolute_time(); + // average attitude for 5 seconds + while(hrt_elapsed_time(&start) < cal_time * 1000000) { + poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + roll_mean += att.roll; + pitch_mean += att.pitch; + counter++; + } + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + + if (counter > (cal_time * cal_hz / 2 )) { + roll_mean /= counter; + pitch_mean /= counter; + } else { + mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); + return 1; + } + + if (fabsf(roll_mean) > 0.8f ) { + mavlink_and_console_log_critical(mavlink_fd, "excess roll angle"); + return 1; + } else if (fabsf(pitch_mean) > 0.8f ) { + mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle"); + return 1; + } + else { + roll_mean *= (float)M_RAD_TO_DEG; + pitch_mean *= (float)M_RAD_TO_DEG; + param_set(roll_offset_handler, &roll_mean); + param_set(pitch_offset_handler, &pitch_mean); + } + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); + return 0; + +} diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6b7e16d449..05c02e294d 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,5 +45,6 @@ #include int do_accel_calibration(int mavlink_fd); +int do_level_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5969de6e83..7e29dbd94d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -317,6 +317,8 @@ int commander_main(int argc, char *argv[]) calib_ret = do_accel_calibration(mavlink_fd); } else if (!strcmp(argv[2], "gyro")) { calib_ret = do_gyro_calibration(mavlink_fd); + } else if (!strcmp(argv[2], "level")) { + calib_ret = do_level_calibration(mavlink_fd); } else { warnx("argument %s unsupported.", argv[2]); } @@ -2720,7 +2722,10 @@ void *commander_low_prio_loop(void *arg) /* accelerometer calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_accel_calibration(mavlink_fd); - + } else if ((int)(cmd.param5) == 2) { + // board offset calibration + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); From 11564a0f14c203811436ddfc8fabce5f7006e1db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 17:03:28 +0200 Subject: [PATCH 17/17] Mission yaw mode: Default to facing the next waypoint --- src/modules/navigator/mission_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 6310cf6de4..b1bd4e540e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -110,4 +110,4 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); * @max 3 * @group Mission */ -PARAM_DEFINE_INT32(MIS_YAWMODE, 0); +PARAM_DEFINE_INT32(MIS_YAWMODE, 1);