From 4e360064d90622fbf93d4d2680f45d210c4a0015 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 15 Jun 2019 20:45:26 -0400 Subject: [PATCH] commander refactor and cleanup offboard control mode --- src/modules/commander/Commander.cpp | 355 ++++++++--------------- src/modules/commander/Commander.hpp | 20 +- src/modules/commander/commander_params.c | 4 +- 3 files changed, 147 insertions(+), 232 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 496558e4e36c..7d3296d3b3f0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -87,7 +87,6 @@ #include #include #include -#include #include #include #include @@ -95,9 +94,7 @@ #include #include #include -#include #include -#include #include typedef enum VEHICLE_MODE_FLAG { @@ -142,8 +139,6 @@ static float min_stick_change = 0.25f; static struct vehicle_status_s status = {}; static struct actuator_armed_s armed = {}; static struct safety_s safety = {}; -static struct vehicle_control_mode_s control_mode = {}; -static struct offboard_control_mode_s offboard_control_mode = {}; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; static struct commander_state_s internal_state = {}; @@ -195,8 +190,6 @@ void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s void get_circuit_breaker_params(); -void set_control_mode(); - bool stabilization_required(); void print_reject_mode(const char *msg); @@ -218,7 +211,7 @@ static int power_button_state_notification_cb(board_power_button_state_notificat { // Note: this can be called from IRQ handlers, so we publish a message that will be handled // on the main thread of commander. - power_button_state_s button_state; + power_button_state_s button_state{}; button_state.timestamp = hrt_absolute_time(); int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; @@ -563,7 +556,6 @@ Commander::Commander() : // We want to accept RC inputs as default status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL; - internal_state.timestamp = hrt_absolute_time(); status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -572,8 +564,6 @@ Commander::Commander() : status_flags.offboard_control_signal_lost = true; status.data_link_lost = true; - status.timestamp = hrt_absolute_time(); - status_flags.condition_power_input_valid = true; status_flags.rc_calibration_valid = true; @@ -1172,8 +1162,6 @@ Commander::run() param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); - param_t _param_offboard_loss_act = param_find("COM_OBL_ACT"); - param_t _param_offboard_loss_rc_act = param_find("COM_OBL_RC_ACT"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -1181,7 +1169,6 @@ Commander::run() param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST"); param_t _param_min_stick_change = param_find("COM_RC_STICK_OV"); param_t _param_geofence_action = param_find("GF_ACTION"); - param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T"); param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS"); param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN"); param_t _param_rc_override = param_find("COM_RC_OVERRIDE"); @@ -1220,8 +1207,7 @@ Commander::run() { // we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen // in IRQ context. - power_button_state_s button_state; - button_state.timestamp = 0; + power_button_state_s button_state{}; button_state.event = 0xff; power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state); @@ -1234,20 +1220,11 @@ Commander::run() get_circuit_breaker_params(); - /* publish initial state */ - _status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* armed topic */ - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); hrt_abstime last_disarmed_timestamp = 0; - /* vehicle control mode topic */ - orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - /* command ack */ orb_advert_t command_ack_pub = nullptr; - orb_advert_t commander_state_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 */ mission_init(); @@ -1267,7 +1244,6 @@ Commander::run() uORB::Subscription cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::Subscription param_changed_sub{ORB_ID(parameter_update)}; uORB::Subscription safety_sub{ORB_ID(safety)}; uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)}; @@ -1335,9 +1311,6 @@ Commander::run() param_get(_param_rc_arm_hyst, &rc_arm_hyst); rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; - float offboard_loss_timeout = 0.0f; - int32_t offboard_loss_act = 0; - int32_t offboard_loss_rc_act = 0; int32_t posctl_nav_loss_act = 0; int32_t geofence_action = 0; int32_t flight_uuid = 0; @@ -1454,9 +1427,6 @@ Commander::run() param_get(_param_geofence_action, &geofence_action); param_get(_param_flight_uuid, &flight_uuid); - param_get(_param_offboard_loss_timeout, &offboard_loss_timeout); - param_get(_param_offboard_loss_act, &offboard_loss_act); - param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act); param_get(_param_arm_switch_is_button, &arm_switch_is_button); param_get(_param_arm_without_gps, &arm_without_gps_param); @@ -1507,69 +1477,7 @@ Commander::run() sp_man_sub.update(&sp_man); - // if this is the first time entering OFFBOARD the subscription may not be active yet - bool force_offboard_update = false; - - if (commander_state_s::MAIN_STATE_OFFBOARD) { - if (offboard_control_mode.timestamp == 0) { - offboard_control_mode_sub.forceInit(); - force_offboard_update = true; - } - } - - if (offboard_control_mode_sub.updated() || force_offboard_update) { - - const offboard_control_mode_s old = offboard_control_mode; - offboard_control_mode_sub.copy(&offboard_control_mode); - - if (old.ignore_thrust != offboard_control_mode.ignore_thrust || - old.ignore_attitude != offboard_control_mode.ignore_attitude || - old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x || - old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y || - old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z || - old.ignore_position != offboard_control_mode.ignore_position || - old.ignore_velocity != offboard_control_mode.ignore_velocity || - old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force || - old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) { - - status_changed = true; - } - } - - if (offboard_control_mode.timestamp != 0 && - offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { - - if (status_flags.offboard_control_signal_lost) { - - status_flags.offboard_control_signal_lost = false; - status_flags.offboard_control_loss_timeout = false; - status_changed = true; - } - - } else { - if (!status_flags.offboard_control_signal_lost) { - status_flags.offboard_control_signal_lost = true; - status_changed = true; - } - - /* check timer if offboard was there but now lost */ - if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) { - if (offboard_loss_timeout < FLT_EPSILON) { - /* execute loss action immediately */ - status_flags.offboard_control_loss_timeout = true; - - } 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(); - } - - if (status_flags.offboard_control_loss_timeout) { - status_changed = true; - } - } - } - + offboard_control_update(status_changed); if (system_power_sub.updated()) { system_power_s system_power = {}; @@ -2349,8 +2257,8 @@ Commander::run() status_flags, land_detector.landed, (link_loss_actions_t)_param_nav_rcl_act.get(), - offboard_loss_act, - offboard_loss_rc_act, + _param_com_obl_act.get(), + _param_com_obl_rc_act.get(), posctl_nav_loss_act); if (status.failsafe != failsafe_old) { @@ -2369,14 +2277,10 @@ Commander::run() /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 1 Hz or immediately when changed */ if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed || nav_state_changed) { - set_control_mode(); - control_mode.timestamp = now; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - - status.timestamp = now; - orb_publish(ORB_ID(vehicle_status), _status_pub, &status); + update_control_mode(); - armed.timestamp = now; + status.timestamp = hrt_absolute_time(); + _status_pub.publish(status); /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ if (safety.safety_switch_available) { @@ -2392,25 +2296,16 @@ Commander::run() armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5_s); } - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + armed.timestamp = hrt_absolute_time(); + _armed_pub.publish(armed); /* publish internal state for logging purposes */ - if (commander_state_pub != nullptr) { - orb_publish(ORB_ID(commander_state), commander_state_pub, &internal_state); - - } else { - commander_state_pub = orb_advertise(ORB_ID(commander_state), &internal_state); - } + internal_state.timestamp = hrt_absolute_time(); + _commander_state_pub.publish(internal_state); /* publish vehicle_status_flags */ status_flags.timestamp = hrt_absolute_time(); - - 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); - } + _vehicle_status_flags_pub.publish(status_flags); } /* play arming and battery warning tunes */ @@ -3158,83 +3053,53 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac } void -set_control_mode() +Commander::update_control_mode() { + vehicle_control_mode_s control_mode{}; + + control_mode.timestamp = hrt_absolute_time(); + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol); - control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = stabilization_required(); control_mode.flag_control_attitude_enabled = stabilization_required(); - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_STAB: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_rattitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = !status.in_transition_mode; control_mode.flag_control_velocity_enabled = !status.in_transition_mode; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: @@ -3251,125 +3116,89 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = !status.in_transition_mode; control_mode.flag_control_velocity_enabled = !status.in_transition_mode; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: /* TODO: check if this makes sense */ - control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_termination_enabled = true; break; - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_offboard_enabled = true; - - /* - * The control flags depend on what is ignored according to the offboard control mode topic - * 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_x || - !offboard_control_mode.ignore_bodyrate_y || - !offboard_control_mode.ignore_bodyrate_z || - !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 || + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: { + + const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); + + control_mode.flag_control_offboard_enabled = true; + + /* + * The control flags depend on what is ignored according to the offboard control mode topic + * 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_x || + !offboard_control_mode.ignore_bodyrate_y || + !offboard_control_mode.ignore_bodyrate_z || + !offboard_control_mode.ignore_attitude || !offboard_control_mode.ignore_position || !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - // TO-DO: Add support for other modes than yawrate control - control_mode.flag_control_yawrate_override_enabled = - offboard_control_mode.ignore_bodyrate_x && - offboard_control_mode.ignore_bodyrate_y && - !offboard_control_mode.ignore_bodyrate_z && - !offboard_control_mode.ignore_attitude; + 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; - control_mode.flag_control_rattitude_enabled = false; + // TO-DO: Add support for other modes than yawrate control + control_mode.flag_control_yawrate_override_enabled = + offboard_control_mode.ignore_bodyrate_x && + offboard_control_mode.ignore_bodyrate_y && + !offboard_control_mode.ignore_bodyrate_z && + !offboard_control_mode.ignore_attitude; + + control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !status.in_transition_mode; + control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && + !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; + 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; - control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !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; - 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_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode && + !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; + control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; + } break; case vehicle_status_s::NAVIGATION_STATE_ORBIT: @@ -3389,6 +3218,8 @@ set_control_mode() default: break; } + + _control_mode_pub.publish(control_mode); } bool @@ -4440,3 +4271,71 @@ void Commander::estimator_check(bool *status_changed) check_valid(lpos.timestamp, _param_com_pos_fs_delay.get() * 1_s, lpos.z_valid, &(status_flags.condition_local_altitude_valid), status_changed); } + +void +Commander::offboard_control_update(bool &status_changed) +{ + const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); + + // if this is the first time entering OFFBOARD the subscription may not be active yet + bool force_update = false; + + if (commander_state_s::MAIN_STATE_OFFBOARD) { + if (offboard_control_mode.timestamp == 0) { + _offboard_control_mode_sub.forceInit(); + force_update = true; + } + } + + if (_offboard_control_mode_sub.updated() || force_update) { + const offboard_control_mode_s old = offboard_control_mode; + + if (_offboard_control_mode_sub.update()) { + if (old.ignore_thrust != offboard_control_mode.ignore_thrust || + old.ignore_attitude != offboard_control_mode.ignore_attitude || + old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x || + old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y || + old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z || + old.ignore_position != offboard_control_mode.ignore_position || + old.ignore_velocity != offboard_control_mode.ignore_velocity || + old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force || + old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) { + + status_changed = true; + } + } + } + + if (offboard_control_mode.timestamp != 0 && + offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status_flags.offboard_control_signal_lost) { + status_flags.offboard_control_signal_lost = false; + status_flags.offboard_control_loss_timeout = false; + status_changed = true; + } + + } else { + if (!status_flags.offboard_control_signal_lost) { + status_flags.offboard_control_signal_lost = true; + status_changed = true; + } + + /* check timer if offboard was there but now lost */ + if (!status_flags.offboard_control_loss_timeout && offboard_control_mode.timestamp != 0) { + if (_param_com_of_loss_t.get() < FLT_EPSILON) { + /* execute loss action immediately */ + status_flags.offboard_control_loss_timeout = true; + + } else { + /* wait for timeout if set */ + status_flags.offboard_control_loss_timeout = offboard_control_mode.timestamp + + OFFBOARD_TIMEOUT + _param_com_of_loss_t.get() * 1e6f < hrt_absolute_time(); + } + + if (status_flags.offboard_control_loss_timeout) { + status_changed = true; + } + } + } + +} diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 79879693f644..e29f54e44ada 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -47,7 +47,9 @@ #include #include #include +#include #include +#include // subscriptions #include @@ -55,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -128,7 +131,11 @@ class Commander : public ModuleBase, public ModuleParams (ParamInt) _airspeed_fail_action, (ParamFloat) _airspeed_stall, (ParamInt) _airspeed_rtl_delay, - (ParamInt) _param_com_flt_profile + (ParamInt) _param_com_flt_profile, + + (ParamFloat) _param_com_of_loss_t, + (ParamInt) _param_com_obl_act, + (ParamInt) _param_com_obl_rc_act ) @@ -187,6 +194,8 @@ class Commander : public ModuleBase, public ModuleParams // Set the system main state based on the current RC inputs transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed); + void update_control_mode(); + void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed); @@ -200,6 +209,8 @@ class Commander : public ModuleBase, public ModuleParams void estimator_check(bool *status_changed); + void offboard_control_update(bool &status_changed); + void airspeed_use_check(); void battery_status_check(); @@ -240,14 +251,19 @@ class Commander : public ModuleBase, public ModuleParams uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::SubscriptionData _sensor_bias_sub{ORB_ID(sensor_bias)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; // Publications + uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; + uORB::Publication _status_pub{ORB_ID(vehicle_status)}; + uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; + uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; + uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; - orb_advert_t _status_pub{nullptr}; }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 57381c628a6d..0bb1a5bf2a15 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -337,7 +337,7 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f); * @value 1 Hold mode * @value 2 Return mode * - * @group Mission + * @group Commander */ PARAM_DEFINE_INT32(COM_OBL_ACT, 0); @@ -353,7 +353,7 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0); * @value 3 Return mode * @value 4 Land mode * @value 5 Hold mode - * @group Mission + * @group Commander */ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);