From e224441ac14b4ed384d77995b7dbdbc047cee6a5 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 10:34:01 -0600 Subject: [PATCH 01/14] special treatment and warning message for HIL platform arming --- src/modules/commander/commander.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 64fd972ba4..c6f5eaa7b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -437,6 +437,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char { transition_result_t arming_res = TRANSITION_NOT_CHANGED; + // For HIL platforms, require that simulated sensors are connected + if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); + return TRANSITION_DENIED; + } + // 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 ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, From 71da3976abf593c7d515b8aa78b49e7f8553a51e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 15:03:03 -0600 Subject: [PATCH 02/14] add HIL autostart ID range macros and remove warnx --- src/modules/commander/commander.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f5eaa7b5..8091a73ece 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,6 +147,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define HIL_ID_MIN 1000 +#define HIL_ID_MAX 1999 + enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -438,7 +441,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX + && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } @@ -1162,7 +1166,11 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id > 1999) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + // HIL configuration selected: real sensors will be disabled + status.condition_system_sensors_initialized = false; + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } else { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune @@ -1170,11 +1178,6 @@ int commander_thread_main(int argc, char *argv[]) else { set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - } else { - // HIL configuration selected: real sensors will be disabled - warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id); - status.condition_system_sensors_initialized = false; - set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } commander_boot_timestamp = hrt_absolute_time(); @@ -1345,14 +1348,14 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id > 1999) { - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); - } else { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { /* HIL configuration: check only RC input */ - warnx("new telemetry link connected: checking RC status"); (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } else { + /* check sensors also */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } } From 7e48c66c221fc281b37dfb0db188440f89823ce6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 10 Jun 2015 11:05:56 -0600 Subject: [PATCH 03/14] add is_hil_setup() --- src/modules/commander/commander.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8091a73ece..becf585452 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -256,6 +256,15 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result); +/** + * check whether autostart ID is in the reserved range for HIL setups + */ +bool is_hil_setup(int id); + +bool is_hil_setup(int id) { + return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); +} + int commander_main(int argc, char *argv[]) { @@ -441,8 +450,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX - && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } @@ -1166,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune @@ -1348,7 +1356,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); From e8a9c200561fea02f24a97883e6c617d07b251ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:30:05 +0200 Subject: [PATCH 04/14] EKF: Ensure we start with zero local altitude and zero GPS offset. Since the filter is not publishing any data at this point this is not relevant in operation, but might be important later if we publish a separate altitude estimate topic --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 74d6dbadf0..50410eac7d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -631,7 +631,10 @@ void AttitudePositionEstimatorEKF::task_main() // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors - _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + + if (_gpsIsGood) { + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + } // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -658,6 +661,8 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + _filter_ref_offset = -_baro.altitude; + warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { From 086123fe8419f9382d0c0e2d80e1fe7a27a17ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:40:39 +0200 Subject: [PATCH 05/14] Fix RC failsafe handling when landed --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 92f73ca21f..7a379612da 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -494,7 +494,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { From 25c23dbf4cb58d4e8c94a749801a1129ef7cdf34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 13:19:36 +0200 Subject: [PATCH 06/14] back out payload mixer from FX79 default config --- ROMFS/px4fmu_common/mixers/FX79.main.mix | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index b8879af9ee..3968a2b08d 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -51,19 +51,3 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - -Inputs to the mixer come from channel group 2 (payload), channels 0 -(bay servo 1), 1 (bay servo 2) and 3 (drop release). ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 From f0f3ffaec1b012fd95d220a9379ae07ed30053e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 13:31:58 +0200 Subject: [PATCH 07/14] IO firmware: Do not apply trim values a second time --- src/modules/px4iofirmware/mixer.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 596064f15f..9fadec57c0 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -311,13 +311,6 @@ mixer_callback(uintptr_t handle, case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); - if (control_group == 0 && control_index == 0) { - control += REG_TO_FLOAT(r_setup_trim_roll); - } else if (control_group == 0 && control_index == 1) { - control += REG_TO_FLOAT(r_setup_trim_pitch); - } else if (control_group == 0 && control_index == 2) { - control += REG_TO_FLOAT(r_setup_trim_yaw); - } break; } return -1; From 3cc2b7ed1285489b4cda8360d720738e0d146e0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 14:51:34 +0200 Subject: [PATCH 08/14] EKF: Add small gyro failover hysteresis --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 50410eac7d..b9897ffcfc 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -72,6 +72,8 @@ static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in secon static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) +static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) +static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) /** * estimator app start / stop handling function @@ -1224,7 +1226,7 @@ void AttitudePositionEstimatorEKF::pollData() if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; @@ -1263,7 +1265,7 @@ void AttitudePositionEstimatorEKF::pollData() int last_accel_main = _accel_main; /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) { + if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; From 000434be15c9ac8a93fa0e675f790464d8b5d23e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 14:51:49 +0200 Subject: [PATCH 09/14] IO mixer: Limit outputs to proper range --- src/modules/px4iofirmware/mixer.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9fadec57c0..5e6a3a585a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,6 +353,13 @@ mixer_callback(uintptr_t handle, return -1; } + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + return 0; } From 9155e8a7fe2a7611c6b1ed136b5691475546a65c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 16:35:10 +0200 Subject: [PATCH 10/14] FX79: Increase travel --- ROMFS/px4fmu_common/mixers/FX79.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix index 3968a2b08d..3223eff133 100755 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ b/ROMFS/px4fmu_common/mixers/FX79.main.mix @@ -27,13 +27,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 9500 9500 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 8500 8500 0 -10000 10000 +S: 0 1 -9500 -9500 0 -10000 10000 Output 2 -------- From f2b81ce69a88683289ead9de33ce83efc3831a1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:22:47 +0200 Subject: [PATCH 11/14] commander: Only update home position if not armed already --- src/modules/commander/commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index becf585452..7c15992f40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -499,10 +499,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state - arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED; + + arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command"); /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (arming_ret == TRANSITION_CHANGED && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); } @@ -2041,11 +2045,11 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp + /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); - //First time home position update - if (!status.condition_home_position_valid) { + /* First time home position update - but only if disarmed */ + if (!status.condition_home_position_valid && !armed.armed) { commander_set_home_position(home_pub, _home, local_position, global_position); } From bc48634101dff97b7e4cd2d5e3008613a1c0df6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:24:26 +0200 Subject: [PATCH 12/14] Navigator: Reject missions with relative altitude if no home was set before arming --- src/modules/navigator/mission.cpp | 2 +- .../navigator/mission_feasibility_checker.cpp | 23 ++++++++++++------- .../navigator/mission_feasibility_checker.h | 8 +++---- src/modules/navigator/navigator.h | 1 + 4 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98b01c7578..4eaeca1d8e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -232,7 +232,7 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt); + _navigator->get_home_position()->alt, _navigator->home_position_valid()); } else { warnx("offboard mission update failed"); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d9297f25bf..19baa79fcd 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { bool failed = false; /* Init if not done yet */ @@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); else - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Perform checks and issue feedback to the user for all checks */ bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resGeofence && resHomeAltitude); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); @@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resLanding && resGeofence && resHomeAltitude); @@ -136,7 +136,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -152,6 +152,13 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + if (missionitem.altitude_is_relative && !home_valid) { + if (throw_error) { + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; + } + } + /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94b6b29226..964bd6097d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -61,16 +61,16 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); public: MissionFeasibilityChecker(); @@ -80,7 +80,7 @@ public: * Returns true if mission is feasible and false otherwise */ bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt); + float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 95e3f9f964..782a297fbb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -134,6 +134,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } + bool home_position_valid() { return _home_position_set; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } From a66b1b9d04c6c200879576e2f4d9609e229606c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 17:02:11 +0200 Subject: [PATCH 13/14] Improve feedback when auto mode is rejected due to a non suitable mission --- src/modules/navigator/mission.cpp | 4 +-- .../navigator/mission_feasibility_checker.cpp | 25 +++++++++---------- .../navigator/mission_feasibility_checker.h | 2 +- src/modules/navigator/navigator_main.cpp | 5 +++- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 4eaeca1d8e..13fcd5daca 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -230,7 +230,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid()); @@ -374,7 +374,7 @@ Mission::set_mission_items() } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); + mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 19baa79fcd..9d1dc7c7e6 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,26 +63,27 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { bool failed = false; /* Init if not done yet */ init(); - /* Open mavlink fd */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } + _mavlink_fd = mavlink_fd; // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - if (isRotarywing) + if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); - else + } else { failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + } + + if (!failed) { + mavlink_log_info(_mavlink_fd, "Mission checked and ready."); + } return !failed; } @@ -152,11 +153,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { - if (throw_error) { - mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); - return false; - } + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; } /* calculate the global waypoint altitude */ @@ -204,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } } - mavlink_log_info(_mavlink_fd, "Mission ready."); return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 964bd6097d..9c9511be3d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -79,7 +79,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 97baa1235f..8cfce50879 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -209,7 +209,10 @@ Navigator::home_position_update() if (updated) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - _home_position_set = true; + + if (_home_pos.timestamp > 0) { + _home_position_set = true; + } } } From e94bc7960c027d8e806e026b3c25d79c378a6fe7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 10 Jun 2015 21:20:57 +0300 Subject: [PATCH 14/14] libuavcan submodule updated --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 6a4f815d42..b05de95995 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,9 +4,9 @@ [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git -[submodule "uavcan"] +[submodule "libuavcan"] path = src/lib/uavcan - url = git://github.com/pavel-kirienko/uavcan.git + url = git://github.com/UAVCAN/libuavcan.git [submodule "Tools/genmsg"] path = Tools/genmsg url = https://github.com/ros/genmsg.git