zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander: Move telemetry logic to separate functions

sbg
acfloria 7 years ago committed by Beat Küng
parent
443a4016af
commit
1124a397f2
2 changed files with 252 additions and 226 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 36
      src/modules/commander/Commander.hpp
  2. 442
      src/modules/commander/commander.cpp

36
src/modules/commander/Commander.hpp
Unescape View File

@ -64,11 +64,7 @@ using uORB::Subscription; @@ -64,11 +64,7 @@ using uORB::Subscription;
class Commander : public ModuleBase<Commander>, 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: @@ -88,10 +84,6 @@ public:
void enable_hil();
private:
// Subscriptions
Subscription<mission_result_s> _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: @@ -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_s> _mission_result_sub;
};
#endif /* COMMANDER_HPP_ */

442
src/modules/commander/commander.cpp
Unescape View File

@ -102,7 +102,6 @@ @@ -102,7 +102,6 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -624,6 +623,20 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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;
}
}
}

Write Preview
Loading…
Cancel
Save