diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e100e6b4b7..2dfe3d8e30 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -302,6 +302,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 @@ -479,22 +491,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 # diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 47a6e4c965..965c0e3e5d 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) { diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 144614d6bc..f036987d7d 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[]); } diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index c4c674bf09..9d38e8179a 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,7 @@ #include #include +#include #include @@ -270,8 +272,38 @@ out: return success; } +static bool gnssCheck(int mavlink_fd) +{ + bool success = true; + + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + //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, 2000) <= 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; +} + 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; @@ -337,6 +369,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/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5d7852816b..1015dfb1b9 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -135,6 +135,7 @@ #include //#include #include +#include #include #include #include @@ -145,6 +146,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -554,3 +556,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 4995374125..298cd0289b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -130,6 +130,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)) @@ -322,6 +324,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]); } @@ -923,6 +927,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); @@ -1124,8 +1129,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 */ @@ -1134,7 +1137,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, true, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1307,7 +1310,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, true, !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1644,19 +1647,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"); @@ -2716,7 +2731,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); @@ -2760,7 +2778,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, 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 7d973abdcb..fc05cce6e9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -739,5 +739,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); } 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 df93b3adf4..dbf40230ba 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 diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index fdb4e18c57..999bcbf008 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -389,8 +389,7 @@ MavlinkFTP::_workList(PayloadHeader* payload) #else case DT_DIR: #endif - // 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 { 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 c40ebdb4ed..47df1b4698 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 diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 2c8af8aee0..8e501fd8ef 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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 34cc56c234..c3ac138ad5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -860,7 +860,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)); 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