diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 09e6103a73..8acf6a243a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -64,11 +64,7 @@ using uORB::Subscription; class Commander : public ModuleBase, public ModuleParams { public: - Commander() : - ModuleParams(nullptr), - _mission_result_sub(ORB_ID(mission_result)) - { - } + Commander(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -88,10 +84,6 @@ public: void enable_hil(); private: - - // Subscriptions - Subscription _mission_result_sub; - bool handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos, const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed); @@ -122,6 +114,32 @@ private: void mission_init(); + /** + * 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); + + /** + * 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); + + // telemetry variables + int _telemetry_subs[ORB_MULTI_MAX_INSTANCES] = {}; + uint64_t _telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES] = {}; + uint64_t _telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES] = {}; + bool _telemetry_preflight_checks_reported[ORB_MULTI_MAX_INSTANCES] = {}; + bool _telemetry_lost[ORB_MULTI_MAX_INSTANCES] = {}; + bool _telemetry_high_latency[ORB_MULTI_MAX_INSTANCES] = {}; + + // publisher + orb_advert_t _vehicle_cmd_pub = nullptr; + + // subscriptions + Subscription _mission_result_sub; + }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25a6fb1726..2075338ede 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -102,7 +102,6 @@ #include #include #include -#include #include #include #include @@ -624,6 +623,20 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co return arming_res; } +Commander::Commander() : + ModuleParams(nullptr), + _mission_result_sub(ORB_ID(mission_result)) +{ + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + _telemetry_subs[i] = -1; + _telemetry_last_heartbeat[i] = 0; + _telemetry_last_dl_loss[i] = 0; + _telemetry_lost[i] = true; + _telemetry_preflight_checks_reported[i] = false; + _telemetry_high_latency[i] = false; + } +} + bool Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local, @@ -1307,7 +1320,6 @@ Commander::run() /* command ack */ orb_advert_t command_ack_pub = nullptr; orb_advert_t commander_state_pub = nullptr; - orb_advert_t vehicle_cmd_pub = nullptr; orb_advert_t vehicle_status_flags_pub = nullptr; /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ @@ -1346,23 +1358,6 @@ Commander::run() int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode)); memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); - /* Subscribe to telemetry status topics */ - int telemetry_subs[ORB_MULTI_MAX_INSTANCES]; - uint64_t telemetry_last_heartbeat[ORB_MULTI_MAX_INSTANCES]; - uint64_t telemetry_last_dl_loss[ORB_MULTI_MAX_INSTANCES]; - bool telemetry_preflight_checks_reported[ORB_MULTI_MAX_INSTANCES]; - bool telemetry_lost[ORB_MULTI_MAX_INSTANCES]; - bool telemetry_high_latency[ORB_MULTI_MAX_INSTANCES]; - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - telemetry_subs[i] = -1; - telemetry_last_heartbeat[i] = 0; - telemetry_last_dl_loss[i] = 0; - telemetry_lost[i] = true; - telemetry_preflight_checks_reported[i] = false; - telemetry_high_latency[i] = false; - } - /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -1712,73 +1707,8 @@ Commander::run() } } - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - - if (telemetry_subs[i] < 0) { - telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i); - } - - orb_check(telemetry_subs[i], &updated); - - if (updated) { - telemetry_status_s telemetry = {}; - orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry); - - /* 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_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000) - || !telemetry_preflight_checks_reported[i]) && - /* 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; - /* flag the checks as reported for this link when we actually report them */ - telemetry_preflight_checks_reported[i] = 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)); - } - - // 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) { - - mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again."); - } - } - - /* 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; - } - - /* set latency type of the telemetry connection */ - if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) { - telemetry_high_latency[i] = true; - } else { - telemetry_high_latency[i] = false; - } - - - if (telemetry.heartbeat_time > 0) { - telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - } - } - } + // poll the telemetry status + poll_telemetry_status(checkAirspeed, &hotplug_timeout); orb_check(system_power_sub, &updated); @@ -2521,137 +2451,8 @@ Commander::run() } } - /* data links check */ - bool have_link = false; - bool high_latency_link_exists = false; - bool have_low_latency_link = false; - int32_t dl_loss_timeout_local = 0; - int32_t dl_regain_timeout_local = 0; - - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (telemetry_high_latency[i]) { - 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_last_heartbeat[i] != 0 && - hrt_elapsed_time(&telemetry_last_heartbeat[i]) < 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_lost[i] && - hrt_elapsed_time(&telemetry_last_dl_loss[i]) > dl_regain_timeout_local * 1e6) { - - /* report a regain */ - if (telemetry_last_dl_loss[i] > 0) { - mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i); - } else if (telemetry_last_dl_loss[i] == 0) { - /* new link */ - } - - /* got link again or new */ - status_flags.condition_system_prearm_error_reported = false; - status_changed = true; - - telemetry_lost[i] = false; - have_link = true; - if (!telemetry_high_latency[i]) { - have_low_latency_link = true; - } - - } else if (!telemetry_lost[i]) { - /* telemetry was healthy also in last iteration - * we don't have to check a timeout */ - have_link = true; - if (!telemetry_high_latency[i]) { - have_low_latency_link = true; - } - } - - } else { - - if (!telemetry_lost[i]) { - /* only reset the timestamp to a different time on state change */ - telemetry_last_dl_loss[i] = hrt_absolute_time(); - - mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i); - telemetry_lost[i] = true; - } - } - } - - if (have_link) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - status.data_link_lost = false; - status_changed = true; - } - - if (status.high_latency_data_link_active && have_low_latency_link) { - // regained a low latency telemetry link, deactivate the high latency link - // to avoid transmitting unnecessary data over that link - status.high_latency_data_link_active = false; - status_changed = true; - mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK"); - - vehicle_command_s vehicle_cmd; - vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; - vehicle_cmd.param1 = 0.0f; - vehicle_cmd.from_external = false; - vehicle_cmd.target_system = status.system_id; - vehicle_cmd.target_component = 0; - - if (vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd); - - } else { - 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 - status.high_latency_data_link_active = true; - status_changed = true; - - vehicle_command_s vehicle_cmd; - vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; - vehicle_cmd.param1 = 1.0f; - vehicle_cmd.from_external = false; - vehicle_cmd.target_system = status.system_id; - vehicle_cmd.target_component = 0; - - if (vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), vehicle_cmd_pub, &vehicle_cmd); - - } else { - vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd); - } - - 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; - } - } + // data link checks which update the status + data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout, datalink_regain_timeout, &status_changed); // engine failure detection // TODO: move out of commander @@ -4288,3 +4089,210 @@ void Commander::mission_init() orb_unadvertise(mission_pub); } } + +void Commander::poll_telemetry_status(bool checkAirspeed, bool *hotplug_timeout) +{ + bool updated = false; + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + + if (_telemetry_subs[i] < 0) { + _telemetry_subs[i] = orb_subscribe_multi(ORB_ID(telemetry_status), i); + } + + orb_check(_telemetry_subs[i], &updated); + + if (updated) { + telemetry_status_s telemetry = {}; + orb_copy(ORB_ID(telemetry_status), _telemetry_subs[i], &telemetry); + + /* 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_last_heartbeat[i] == 0 || (hrt_elapsed_time(&_telemetry_last_heartbeat[i]) > 3 * 1000 * 1000) + || !_telemetry_preflight_checks_reported[i]) && + /* 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; + /* flag the checks as reported for this link when we actually report them */ + _telemetry_preflight_checks_reported[i] = *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)); + } + + // 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) { + + mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again."); + } + } + + /* 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; + } + + /* set latency type of the telemetry connection */ + if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM) { + _telemetry_high_latency[i] = true; + } else { + _telemetry_high_latency[i] = false; + } + + if (telemetry.heartbeat_time > 0) { + _telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } + } + } +} + +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) +{ + /* data links check */ + bool have_link = false; + bool high_latency_link_exists = false; + bool have_low_latency_link = false; + int32_t dl_loss_timeout_local = 0; + int32_t dl_regain_timeout_local = 0; + + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_telemetry_high_latency[i]) { + 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_last_heartbeat[i] != 0 && + hrt_elapsed_time(&_telemetry_last_heartbeat[i]) < 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_lost[i] && + hrt_elapsed_time(&_telemetry_last_dl_loss[i]) > dl_regain_timeout_local * 1e6) { + + /* report a regain */ + if (_telemetry_last_dl_loss[i] > 0) { + mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i); + } else if (_telemetry_last_dl_loss[i] == 0) { + /* new link */ + } + + /* got link again or new */ + status_flags.condition_system_prearm_error_reported = false; + *status_changed = true; + + _telemetry_lost[i] = false; + have_link = true; + if (!_telemetry_high_latency[i]) { + have_low_latency_link = true; + } + + } else if (!_telemetry_lost[i]) { + /* telemetry was healthy also in last iteration + * we don't have to check a timeout */ + have_link = true; + if (!_telemetry_high_latency[i]) { + have_low_latency_link = true; + } + } + + } else { + + if (!_telemetry_lost[i]) { + /* only reset the timestamp to a different time on state change */ + _telemetry_last_dl_loss[i] = hrt_absolute_time(); + + mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i); + _telemetry_lost[i] = true; + } + } + } + + if (have_link) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + status.data_link_lost = false; + *status_changed = true; + } + + if (status.high_latency_data_link_active && have_low_latency_link) { + // regained a low latency telemetry link, deactivate the high latency link + // to avoid transmitting unnecessary data over that link + status.high_latency_data_link_active = false; + *status_changed = true; + mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK"); + + vehicle_command_s vehicle_cmd; + vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; + vehicle_cmd.param1 = 0.0f; + vehicle_cmd.from_external = false; + vehicle_cmd.target_system = status.system_id; + vehicle_cmd.target_component = 0; + + if (_vehicle_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd); + + } else { + _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 + status.high_latency_data_link_active = true; + *status_changed = true; + + vehicle_command_s vehicle_cmd; + vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; + vehicle_cmd.param1 = 1.0f; + vehicle_cmd.from_external = false; + vehicle_cmd.target_system = status.system_id; + vehicle_cmd.target_component = 0; + + if (_vehicle_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd); + + } else { + _vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd); + } + + 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; + } + } +}