From 7bc3a12efe7f95f66e968d506829f1488e63480e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 Apr 2018 01:09:56 -0400 Subject: [PATCH] commander move telemetry checks to new preflightcheck helper --- src/modules/commander/Commander.hpp | 4 +- src/modules/commander/commander.cpp | 411 +++++++++++++++++----------- 2 files changed, 249 insertions(+), 166 deletions(-) diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index b87f3f027b..50db3a7511 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -121,13 +121,13 @@ private: * Update the telemetry status and the corresponding status variables. * Perform system checks when new telemetry link connected. */ - void poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout); + void poll_telemetry_status(); /** * Checks the status of all available data links and handles switching between different system telemetry states. */ void data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout, - int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed); + int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed); // telemetry variables struct telemetry_data { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e86f1529ff..7fd16e27a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -705,25 +705,32 @@ Commander::handle_command(vehicle_status_s *status_local, case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: if (status_flags.condition_auto_mission_available) { main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); } else { main_ret = TRANSITION_DENIED; } + break; + case PX4_CUSTOM_SUB_MODE_AUTO_RTL: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_LAND: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags, &internal_state); break; + case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags, &internal_state); break; @@ -770,6 +777,7 @@ Commander::handle_command(vehicle_status_s *status_local, } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { /* STABILIZED */ main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state); + } else { /* MANUAL */ main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); @@ -815,11 +823,11 @@ Commander::handle_command(vehicle_status_s *status_local, // Refuse to arm if in manual with non-zero throttle if (cmd_arms - && (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL + && (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE) - && (sp_man.z > 0.1f)) { + && (sp_man.z > 0.1f)) { mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Manual throttle non-zero."); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; @@ -837,8 +845,8 @@ Commander::handle_command(vehicle_status_s *status_local, /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arms && (arming_res == TRANSITION_CHANGED) && - (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && - !home->manual_home) { + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && + !home->manual_home) { set_home_position(*home_pub, *home, local_pos, global_pos, false); } @@ -1047,10 +1055,11 @@ Commander::handle_command(vehicle_status_s *status_local, } break; case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { - // only send the acknowledge from the commander, the command actually is handled by each mavlink instance - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - } - break; + // only send the acknowledge from the commander, the command actually is handled by each mavlink instance + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: @@ -1106,8 +1115,8 @@ Commander::handle_command(vehicle_status_s *status_local, **/ bool Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - bool set_alt_only_to_lpos_ref) + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + bool set_alt_only_to_lpos_ref) { if (!set_alt_only_to_lpos_ref) { //Need global and local position fix to be able to set home @@ -1532,6 +1541,7 @@ Commander::run() if (!armed.armed) { if (param_get(_param_sys_type, &system_type) != OK) { PX4_ERR("failed getting new system type"); + } else { status.system_type = (uint8_t)system_type; } @@ -1629,6 +1639,7 @@ Commander::run() if (updated) { power_button_state_s button_state; orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state); + if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { px4_shutdown_request(false, false); } @@ -1669,7 +1680,7 @@ Commander::run() } else { /* wait for timeout if set */ status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp + - OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time(); + OFFBOARD_TIMEOUT + offboard_loss_timeout * 1e6f < hrt_absolute_time(); } if (status_flags.offboard_control_loss_timeout) { @@ -1679,11 +1690,12 @@ Commander::run() } // poll the telemetry status - poll_telemetry_status(checkAirspeed, &hotplug_timeout); + poll_telemetry_status(); orb_check(system_power_sub, &updated); if (updated) { + system_power_s system_power = {}; orb_copy(ORB_ID(system_power), system_power_sub, &system_power); if (hrt_elapsed_time(&system_power.timestamp) < 200000) { @@ -1715,15 +1727,17 @@ Commander::run() if (updated) { bool previous_safety_off = safety.safety_off; + if (orb_copy(ORB_ID(safety), safety_sub, &safety) == PX4_OK) { /* disarm if safety is now on and still armed */ if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF) - && safety.safety_switch_available && !safety.safety_off) { + && safety.safety_switch_available && !safety.safety_off) { - if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) - ) { + if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, + &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, + &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)) + ) { status_changed = true; } } @@ -1788,6 +1802,7 @@ Commander::run() /* update global position estimate and check for timeout */ bool gpos_updated = false; orb_check(global_position_sub, &gpos_updated); + if (gpos_updated) { orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); gpos_last_update_time_us = hrt_absolute_time(); @@ -1806,21 +1821,26 @@ Commander::run() if (run_quality_checks && status.is_rotary_wing) { bool estimator_status_updated = false; orb_check(estimator_status_sub, &estimator_status_updated); + if (estimator_status_updated) { orb_copy(ORB_ID(estimator_status), estimator_status_sub, &estimator_status); + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { // reset flags and timer time_at_takeoff = hrt_absolute_time(); nav_test_failed = false; nav_test_passed = false; + } else if (land_detector.landed) { // record time of takeoff time_at_takeoff = hrt_absolute_time(); + } else { // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - bool sufficient_time = (hrt_absolute_time() - time_at_takeoff > 30*1000*1000); - bool sufficient_speed = local_position.vx*local_position.vx + local_position.vy*local_position.vy > 25.0f; + bool sufficient_time = (hrt_absolute_time() - time_at_takeoff > 30 * 1000 * 1000); + bool sufficient_speed = local_position.vx * local_position.vx + local_position.vy * local_position.vy > 25.0f; bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f; + if (!nav_test_failed) { if (!nav_test_passed) { // pass if sufficient time or speed @@ -1834,7 +1854,7 @@ Commander::run() } // if the innovation test has failed continuously, declare the nav as failed - if ((hrt_absolute_time() - time_last_innov_pass) > 1000*1000) { + if ((hrt_absolute_time() - time_last_innov_pass) > 1000 * 1000) { nav_test_failed = true; mavlink_log_emergency(&mavlink_log_pub, "CRITICAL NAVIGATION FAILURE - CHECK SENSOR CALIBRATION"); } @@ -1850,6 +1870,7 @@ Commander::run() // If nav is failed, then declare local position and velocity as invalid if (nav_test_failed) { status_flags.condition_global_position_valid = false; + } else { // use global position message to determine validity check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed); @@ -1870,6 +1891,7 @@ Commander::run() if (nav_test_failed) { status_flags.condition_local_position_valid = false; status_flags.condition_local_velocity_valid = false; + } else { // use local position message to determine validity check_posvel_validity(local_position.xy_valid, local_position.eph, eph_threshold, local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, &status_changed); @@ -1883,6 +1905,7 @@ Commander::run() /* Update land detector */ orb_check(land_detector_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); @@ -1891,6 +1914,7 @@ Commander::run() if (was_landed != land_detector.landed) { if (land_detector.landed) { mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected"); + } else { mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); have_taken_off_since_arming = true; @@ -1927,6 +1951,7 @@ Commander::run() // Check for auto-disarm if (armed.armed && land_detector.landed && disarm_when_landed > 0) { auto_disarm_hysteresis.set_state_and_update(true); + } else { auto_disarm_hysteresis.set_state_and_update(false); } @@ -1941,8 +1966,8 @@ Commander::run() main_state_before_rtl = internal_state.main_state; } else if (internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER + && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { // reset flag again when we switched out of it warning_action_on = false; } @@ -1966,9 +1991,12 @@ Commander::run() /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery.warning == battery_status_s::BATTERY_WARNING_LOW && !low_battery_voltage_actions_done) { + low_battery_voltage_actions_done = true; + if (armed.armed) { mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED"); + } else { mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED"); } @@ -1976,6 +2004,7 @@ Commander::run() status_changed = true; } else if (battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL && !critical_battery_voltage_actions_done) { + critical_battery_voltage_actions_done = true; if (!armed.armed) { @@ -2010,16 +2039,19 @@ Commander::run() } else if (battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY && !emergency_battery_voltage_actions_done) { + emergency_battery_voltage_actions_done = true; if (!armed.armed) { mavlink_log_critical(&mavlink_log_pub, "DANGEROUSLY LOW BATTERY, SHUT SYSTEM DOWN"); usleep(200000); int ret_val = px4_shutdown_request(false, false); + if (ret_val) { mavlink_log_critical(&mavlink_log_pub, "SYSTEM DOES NOT SUPPORT SHUTDOWN"); + } else { - while(1) { usleep(1); } + while (1) { usleep(1); } } } else { @@ -2083,8 +2115,8 @@ Commander::run() if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_DENIED) { /* do not complain if not allowed into standby */ @@ -2094,8 +2126,9 @@ Commander::run() /* start mission result check */ const auto prev_mission_instance_count = _mission_result_sub.get().instance_count; + if (_mission_result_sub.update()) { - const mission_result_s& mission_result = _mission_result_sub.get(); + const mission_result_s &mission_result = _mission_result_sub.get(); // if mission_result is valid for the current mission const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp) && (mission_result.instance_count > 0); @@ -2115,14 +2148,16 @@ Commander::run() /* Only evaluate mission state if home is set */ if (status_flags.condition_home_position_valid && - (prev_mission_instance_count != mission_result.instance_count)) { + (prev_mission_instance_count != mission_result.instance_count)) { if (!status_flags.condition_auto_mission_available) { /* the mission is invalid */ tune_mission_fail(true); + } else if (mission_result.warning) { /* the mission has a warning */ tune_mission_fail(true); + } else { /* the mission is valid */ tune_mission_ok(true); @@ -2148,6 +2183,7 @@ Commander::run() if (geofence_result.geofence_violated) { static hrt_abstime last_geofence_violation = 0; const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds + if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) { last_geofence_violation = hrt_absolute_time(); @@ -2165,12 +2201,14 @@ Commander::run() if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state)) { geofence_loiter_on = true; } + break; } case (geofence_result_s::GF_ACTION_RTL) : { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state)) { geofence_rtl_on = true; } + break; } case (geofence_result_s::GF_ACTION_TERMINATE) : { @@ -2186,15 +2224,15 @@ Commander::run() // reset if no longer in LOITER or if manually switched to LOITER geofence_loiter_on = geofence_loiter_on - && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) - && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF - || sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE); + && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) + && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF + || sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE); // reset if no longer in RTL or if manually switched to RTL geofence_rtl_on = geofence_rtl_on - && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL) - && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF - || sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE); + && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL) + && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF + || sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE); warning_action_on = warning_action_on || (geofence_loiter_on || geofence_rtl_on); } @@ -2202,19 +2240,19 @@ Commander::run() // revert geofence failsafe transition if sticks are moved and we were previously in a manual mode // but only if not in a low battery handling action if (rc_override != 0 && !critical_battery_voltage_actions_done && (warning_action_on && - (main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL || - main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL || - main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL || - main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO || - main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE || - main_state_before_rtl == commander_state_s::MAIN_STATE_STAB))) { + (main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL || + main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL || + main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL || + main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO || + main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE || + main_state_before_rtl == commander_state_s::MAIN_STATE_STAB))) { // transition to previous state if sticks are touched if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { + ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); @@ -2225,16 +2263,16 @@ Commander::run() // abort landing or auto or loiter if sticks are moved significantly // but only if not in a low battery handling action if (rc_override != 0 && !critical_battery_voltage_actions_done && - (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)) { + (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)) { // transition to previous state if sticks are touched if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { + ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); @@ -2272,7 +2310,7 @@ Commander::run() } else { if (status.rc_signal_lost) { mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", - (hrt_absolute_time() - rc_signal_lost_timestamp) / 1000); + (hrt_absolute_time() - rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -2280,7 +2318,8 @@ Commander::run() status.rc_signal_lost = false; const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool arm_button_pressed = arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON; + const bool arm_button_pressed = arm_switch_is_button == 1 + && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON; /* DISARM * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm @@ -2292,23 +2331,26 @@ Commander::run() sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; if (in_armed_state && - status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && - (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition) ) { + status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && + (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && + (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL && - internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && - internal_state.main_state != commander_state_s::MAIN_STATE_STAB && - internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && - !land_detector.landed) { + internal_state.main_state != commander_state_s::MAIN_STATE_ACRO && + internal_state.main_state != commander_state_s::MAIN_STATE_STAB && + internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE && + !land_detector.landed) { print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { - arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } + stick_off_counter++; - /* do not reset the counter when holding the arm button longer than needed */ + /* do not reset the counter when holding the arm button longer than needed */ + } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { stick_off_counter = 0; } @@ -2322,8 +2364,8 @@ Commander::run() sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON; if (!in_armed_state && - status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition) ) { + status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && + (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) { if ((stick_on_counter == rc_arm_hyst && stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) { /* we check outside of the transition function here because the requirement @@ -2332,21 +2374,22 @@ Commander::run() */ if ((internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) - && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB) - && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) - && (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) - ) { + && (internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) + && (internal_state.main_state != commander_state_s::MAIN_STATE_STAB) + && (internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) + && (internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) + && (internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) + ) { print_reject_arm("NOT ARMING: Switch to a manual mode first."); } else if (!status_flags.condition_home_position_valid && - geofence_action == geofence_result_s::GF_ACTION_RTL) { + geofence_action == geofence_result_s::GF_ACTION_RTL) { print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, + true /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret != TRANSITION_CHANGED) { usleep(100000); @@ -2354,8 +2397,10 @@ Commander::run() } } } + stick_on_counter++; - /* do not reset the counter when holding the arm button longer than needed */ + /* do not reset the counter when holding the arm button longer than needed */ + } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { stick_on_counter = 0; } @@ -2397,6 +2442,7 @@ Commander::run() status_changed = true; armed.manual_lockdown = true; } + } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { if (armed.manual_lockdown) { mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF"); @@ -2404,7 +2450,9 @@ Commander::run() armed.manual_lockdown = false; } } + /* no else case: do not change lockdown flag in unconfigured case */ + } else { if (!status_flags.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); @@ -2435,13 +2483,13 @@ Commander::run() const float current2throttle = battery.current_a / throttle; if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres)) - || status.engine_failure) { + || status.engine_failure) { const float elapsed = hrt_elapsed_time(×tamp_engine_healthy) / 1e6f; /* potential failure, measure time */ if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres) - && !status.engine_failure) { + && !status.engine_failure) { status.engine_failure = true; status_changed = true; @@ -2466,11 +2514,11 @@ Commander::run() * as finished even though we only just started with the takeoff. Therefore, we also * check the timestamp of the mission_result topic. */ if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - && (_mission_result_sub.get().timestamp > internal_state.timestamp) - && _mission_result_sub.get().finished) { + && (_mission_result_sub.get().timestamp > internal_state.timestamp) + && _mission_result_sub.get().finished) { const bool mission_available = (_mission_result_sub.get().timestamp > commander_boot_timestamp) - && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; + && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; if ((takeoff_complete_act == 1) && mission_available) { main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); @@ -2488,7 +2536,7 @@ Commander::run() * we can as well just wait in a hold mode which enables tablet control. */ if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) - && status_flags.condition_home_position_valid) { + && status_flags.condition_home_position_valid) { main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); } @@ -2570,11 +2618,12 @@ Commander::run() if (!_home.manual_home) { if (armed.armed) { if ((!was_armed || (was_landed && !land_detector.landed)) && - (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ set_home_position(home_pub, _home, local_position, global_position, false); } + } else { if (status_flags.condition_home_position_valid) { if (land_detector.landed && local_position.xy_valid && local_position.z_valid) { @@ -2582,8 +2631,8 @@ Commander::run() float home_dist_xy = -1.0f; float home_dist_z = -1.0f; mavlink_wpm_distance_to_point_local(_home.x, _home.y, _home.z, - local_position.x, local_position.y, local_position.z, - &home_dist_xy, &home_dist_z); + local_position.x, local_position.y, local_position.z, + &home_dist_xy, &home_dist_z); if ((home_dist_xy > local_position.eph * 2) || (home_dist_z > local_position.epv * 2)) { @@ -2591,6 +2640,7 @@ Commander::run() set_home_position(home_pub, _home, local_position, global_position, false); } } + } else { /* First time home position update - but only if disarmed */ set_home_position(home_pub, _home, local_position, global_position, false); @@ -2621,21 +2671,20 @@ Commander::run() /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, - &armed, - &internal_state, - &mavlink_log_pub, - (link_loss_actions_t)datalink_loss_act, - _mission_result_sub.get().finished, - _mission_result_sub.get().stay_in_failsafe, - status_flags, - land_detector.landed, - (link_loss_actions_t)rc_loss_act, - offboard_loss_act, - offboard_loss_rc_act, - posctl_nav_loss_act); - - if (status.failsafe != failsafe_old) - { + &armed, + &internal_state, + &mavlink_log_pub, + (link_loss_actions_t)datalink_loss_act, + _mission_result_sub.get().finished, + _mission_result_sub.get().stay_in_failsafe, + status_flags, + land_detector.landed, + (link_loss_actions_t)rc_loss_act, + offboard_loss_act, + offboard_loss_rc_act, + posctl_nav_loss_act); + + if (status.failsafe != failsafe_old) { status_changed = true; if (status.failsafe) { @@ -2670,6 +2719,7 @@ Commander::run() /* safety is off, go into prearmed */ armed.prearmed = safety.safety_off; + } else { /* safety is not present, go into prearmed * (all output drivers should be started / unlocked last in the boot process @@ -2677,6 +2727,7 @@ Commander::run() */ armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000); } + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); /* publish internal state for logging purposes */ @@ -2692,6 +2743,7 @@ Commander::run() if (vehicle_status_flags_pub != nullptr) { orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &status_flags); + } else { vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &status_flags); } @@ -2717,6 +2769,7 @@ Commander::run() } else if (status.failsafe) { tune_failsafe(true); + } else { set_tune(TONE_STOP_TUNE); } @@ -2735,7 +2788,8 @@ Commander::run() /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); - if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout)) { + if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized + && status_flags.condition_system_hotplug_timeout)) { set_tune_override(TONE_GPS_WARNING_TUNE); sensor_fail_tune_played = true; status_changed = true; @@ -2824,7 +2878,7 @@ Commander::check_valid(const hrt_abstime& timestamp, const hrt_abstime& timeout, void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, - bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local) + bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local) { static hrt_abstime overload_start = 0; @@ -2832,6 +2886,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (overload_start == 0 && overload) { overload_start = hrt_absolute_time(); + } else if (!overload) { overload_start = 0; } @@ -2881,8 +2936,10 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_LOW) { led_color = led_control_s::COLOR_AMBER; + } else if (battery_local->warning == battery_status_s::BATTERY_WARNING_CRITICAL) { led_color = led_control_s::COLOR_RED; + } else { if (status_flags.condition_home_position_valid && status_flags.condition_global_position_valid) { led_color = led_control_s::COLOR_GREEN; @@ -2892,6 +2949,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } } } + if (led_mode != led_control_s::MODE_OFF) { rgbled_set_color_and_mode(led_color, led_mode); } @@ -2905,9 +2963,11 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (actuator_armed->armed) { if (status.failsafe) { led_off(LED_BLUE); + if (leds_counter % 5 == 0) { led_toggle(LED_GREEN); } + } else { led_off(LED_GREEN); @@ -2917,6 +2977,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (actuator_armed->ready_to_arm) { led_off(LED_BLUE); + /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) { led_toggle(LED_GREEN); @@ -2924,6 +2985,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else { led_off(LED_BLUE); + /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) { led_toggle(LED_GREEN); @@ -2946,17 +3008,19 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -Commander::set_main_state(const vehicle_status_s& status_local, const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) +Commander::set_main_state(const vehicle_status_s &status_local, const vehicle_global_position_s &global_position, + const vehicle_local_position_s &local_position, bool *changed) { if (safety.override_available && safety.override_enabled) { return set_main_state_override_on(status_local, changed); + } else { return set_main_state_rc(status_local, global_position, local_position, changed); } } transition_result_t -Commander::set_main_state_override_on(const vehicle_status_s& status_local, bool *changed) +Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed) { transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); *changed = (res == TRANSITION_CHANGED); @@ -2965,7 +3029,8 @@ Commander::set_main_state_override_on(const vehicle_status_s& status_local, bool } transition_result_t -Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) +Commander::set_main_state_rc(const vehicle_status_s &status_local, const vehicle_global_position_s &global_position, + const vehicle_local_position_s &local_position, bool *changed) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; @@ -2976,8 +3041,8 @@ Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle /* manual setpoint has not updated, do not re-evaluate it */ if (!(!_last_condition_global_position_valid && - status_flags.condition_global_position_valid) - && (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || + status_flags.condition_global_position_valid) + && (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || ((_last_sp_man.offboard_switch == sp_man.offboard_switch) && (_last_sp_man.return_switch == sp_man.return_switch) && (_last_sp_man.mode_switch == sp_man.mode_switch) && @@ -2994,7 +3059,7 @@ Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle // the sticks to break out of the autonomous state if (!warning_action_on - && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL || internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || @@ -3210,7 +3275,7 @@ Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE && - sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { + sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { /* * Legacy mode: * Acro switch being used as stabilized switch in FW. @@ -3343,7 +3408,8 @@ Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle } void -Commander::reset_posvel_validity(const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) +Commander::reset_posvel_validity(const vehicle_global_position_s &global_position, + const vehicle_local_position_s &local_position, bool *changed) { // reset all the check probation times back to the minimum value gpos_probation_time_us = POSVEL_PROBATION_MIN; @@ -3357,7 +3423,9 @@ Commander::reset_posvel_validity(const vehicle_global_position_s& global_positio } bool -Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime& data_timestamp_us, hrt_abstime* last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed) +Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, + const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, + bool *validity_changed) { const bool was_valid = *valid_state; bool valid = was_valid; @@ -3378,17 +3446,20 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac // still valid, continue to decrease probation time const int64_t probation_time_new = *probation_time_us - hrt_elapsed_time(last_fail_time_us); *probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX); + } else { // check if probation period has elapsed if (hrt_elapsed_time(last_fail_time_us) > *probation_time_us) { valid = true; } } + } else { // level check failed if (was_valid) { // FAILURE! no longer valid valid = false; + } else { // failed again, increase probation time const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) * posctl_nav_loss_gain; @@ -3490,7 +3561,8 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: /* override is not ok for the RTL and recovery mode */ control_mode.flag_external_manual_override_ok = false; - /* fallthrough */ + + /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: @@ -3580,33 +3652,33 @@ set_control_mode() * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) */ control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate || - !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !status.in_transition_mode; + !status.in_transition_mode; control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; + !offboard_control_mode.ignore_position) && !status.in_transition_mode && + !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; + !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; + !control_mode.flag_control_acceleration_enabled; control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; + !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; break; @@ -3621,7 +3693,7 @@ stabilization_required() return (status.is_rotary_wing || // is a rotary wing, or status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND - !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode + !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode } void @@ -3694,7 +3766,8 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); } else { - command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); + command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); } } @@ -3724,6 +3797,7 @@ void *commander_low_prio_loop(void *arg) /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("commander: poll error %d, %d", pret, errno); continue; + } else if (pret != 0) { struct vehicle_command_s cmd; @@ -3780,11 +3854,12 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { + false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); break; + } else { status_flags.condition_calibration_enabled = true; } @@ -3795,8 +3870,8 @@ void *commander_low_prio_loop(void *arg) calib_ret = do_gyro_calibration(&mavlink_log_pub); } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || - (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || - (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + (int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || + (int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { /* temperature calibration: handled in events module */ break; @@ -3826,10 +3901,12 @@ void *commander_low_prio_loop(void *arg) /* accelerometer calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); calib_ret = do_accel_calibration(&mavlink_log_pub); + } else if ((int)(cmd.param5) == 2) { // board offset calibration answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); calib_ret = do_level_calibration(&mavlink_log_pub); + } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) /* airspeed calibration */ @@ -3848,9 +3925,11 @@ void *commander_low_prio_loop(void *arg) status_flags.rc_input_blocked = false; mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN"); } + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); /* this always succeeds */ calib_ret = OK; + } else { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); } @@ -3862,8 +3941,9 @@ void *commander_low_prio_loop(void *arg) Commander::preflight_check(false); - arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, + false /* fRunPreArmChecks */, + &mavlink_log_pub, &status_flags, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } else { tune_negative(true); @@ -3924,6 +4004,7 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub); } + } else if (((int)(cmd.param1)) == 2) { /* reset parameters and save empty file */ @@ -4010,6 +4091,7 @@ void Commander::mission_init() { /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_s mission = {}; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { if (mission.count > 0) { @@ -4034,16 +4116,18 @@ bool Commander::preflight_check(bool report) { const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); - bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, hrt_elapsed_time(&commander_boot_timestamp)); + bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, + hrt_elapsed_time(&commander_boot_timestamp)); status_flags.condition_system_sensors_initialized = success; return success; } -void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) +void Commander::poll_telemetry_status() { bool updated = false; + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_telemetry[i].subscriber < 0) { @@ -4058,36 +4142,25 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) /* perform system checks when new telemetry link connected */ if (/* we first connect a link or re-connect a link after loosing it or haven't yet reported anything */ - (_telemetry[i].last_heartbeat == 0 || (hrt_elapsed_time(&_telemetry[i].last_heartbeat) > 3 * 1000 * 1000) - || !_telemetry[i].preflight_checks_reported) && - /* and this link has a communication partner */ - (telemetry.heartbeat_time > 0) && - /* and it is still connected */ - (hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) && - /* and the system is not already armed (and potentially flying) */ - !armed.armed) { - - *hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + (_telemetry[i].last_heartbeat == 0 || (hrt_elapsed_time(&_telemetry[i].last_heartbeat) > 3 * 1000 * 1000) + || !_telemetry[i].preflight_checks_reported) && + /* and this link has a communication partner */ + (telemetry.heartbeat_time > 0) && + /* and it is still connected */ + (hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) && + /* and the system is not already armed (and potentially flying) */ + !armed.armed) { + /* flag the checks as reported for this link when we actually report them */ - _telemetry[i].preflight_checks_reported = *hotplug_timeout; - - /* provide RC and sensor status feedback to the user */ - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - /* HITL configuration: check only RC input */ - Preflight::preflightCheck(&mavlink_log_pub, false, false, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, - true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp)); - } else { - /* check sensors also */ - Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, - true, is_vtol(&status), *hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); - } + _telemetry[i].preflight_checks_reported = status_flags.condition_system_hotplug_timeout; + + preflight_check(true); // Provide feedback on mission state - const mission_result_s& mission_result = _mission_result_sub.get(); - if ((mission_result.timestamp > commander_boot_timestamp) && *hotplug_timeout && - (mission_result.instance_count > 0) && !mission_result.valid) { + const mission_result_s &mission_result = _mission_result_sub.get(); + + if ((mission_result.timestamp > commander_boot_timestamp) && status_flags.condition_system_hotplug_timeout && + (mission_result.instance_count > 0) && !mission_result.valid) { mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again."); } @@ -4095,12 +4168,13 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) /* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */ if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) { - _usb_telemetry_active = true; + status_flags.usb_connected = true; } /* set latency type of the telemetry connection */ if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) { _telemetry[i].high_latency = true; + } else { _telemetry[i].high_latency = false; } @@ -4113,7 +4187,7 @@ void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) } void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32_t highlatencydatalink_regain_timeout, - int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed) + int32_t datalink_loss_timeout, int32_t datalink_regain_timeout, bool *status_changed) { /* data links check */ bool have_link = false; @@ -4125,40 +4199,44 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_telemetry[i].high_latency) { high_latency_link_exists = true; + if (status.high_latency_data_link_active) { dl_loss_timeout_local = highlatencydatalink_loss_timeout; dl_regain_timeout_local = highlatencydatalink_regain_timeout; + } else { // if the high latency link is inactive we do not want to accidentally detect it as an active link dl_loss_timeout_local = INT32_MIN; dl_regain_timeout_local = INT32_MAX; } + } else { dl_loss_timeout_local = datalink_loss_timeout; dl_regain_timeout_local = datalink_regain_timeout; } if (_telemetry[i].last_heartbeat != 0 && - hrt_elapsed_time(&_telemetry[i].last_heartbeat) < dl_loss_timeout_local * 1e6) { + hrt_elapsed_time(&_telemetry[i].last_heartbeat) < dl_loss_timeout_local * 1e6) { /* handle the case where data link was gained first time or regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (_telemetry[i].lost && - hrt_elapsed_time(&_telemetry[i].last_dl_loss) > dl_regain_timeout_local * 1e6) { + hrt_elapsed_time(&_telemetry[i].last_dl_loss) > dl_regain_timeout_local * 1e6) { /* report a regain */ if (_telemetry[i].last_dl_loss > 0) { mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i); + } else if (_telemetry[i].last_dl_loss == 0) { /* new link */ } /* got link again or new */ - status_flags.condition_system_prearm_error_reported = false; *status_changed = true; _telemetry[i].lost = false; have_link = true; + if (!_telemetry[i].high_latency) { have_low_latency_link = true; } @@ -4167,6 +4245,7 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; + if (!_telemetry[i].high_latency) { have_low_latency_link = true; } @@ -4213,6 +4292,7 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 _vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd); } } + } else { if (high_latency_link_exists && !status.high_latency_data_link_active && armed.armed) { // low latency telemetry lost and high latency link existing @@ -4243,13 +4323,16 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 if (!status.data_link_lost) { mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK"); + } else { mavlink_log_critical(&mavlink_log_pub, "ACTIVATING AVAILABLE HIGH LATENCY LINK"); } + } else if (!status.data_link_lost) { if (armed.armed) { mavlink_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST"); } + status.data_link_lost = true; status.data_link_lost_counter++; *status_changed = true;