yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander refactor and cleanup offboard control mode

Browse Source
This commit is contained in:
Daniel Agar
2019-06-15 20:45:26 -04:00
committed by GitHub
parent 648e7de249
commit 4e360064d9
3 changed files with 145 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files

351
src/modules/commander/Commander.cpp
View File

@@ -87,7 +87,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/safety.h>
@@ -95,9 +94,7 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vtol_vehicle_status.h>
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);
update_control_mode();
status.timestamp = now;
orb_publish(ORB_ID(vehicle_status), _status_pub, &status);
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;
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
/*
* 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;
const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get();
control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
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_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
!status.in_transition_mode;
control_mode.flag_control_rattitude_enabled = false;
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_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
!status.in_transition_mode;
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_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_position_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;
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_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;
}
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;
}
}
}
}

20
src/modules/commander/Commander.hpp
View File

@@ -47,7 +47,9 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
// subscriptions
#include <uORB/Subscription.hpp>
@@ -55,6 +57,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_command.h>
@@ -128,7 +131,11 @@ private:
(ParamInt<px4::params::COM_ASPD_FS_ACT>) _airspeed_fail_action,
(ParamFloat<px4::params::COM_ASPD_STALL>) _airspeed_stall,
(ParamInt<px4::params::COM_ASPD_FS_DLY>) _airspeed_rtl_delay,
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act
)
@@ -187,6 +194,8 @@ private:
// 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 &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
bool *changed);
@@ -200,6 +209,8 @@ private:
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 @@ private:
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
uORB::SubscriptionData<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::SubscriptionData<sensor_bias_s> _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
// Publications
uORB::Publication<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
orb_advert_t _status_pub{nullptr};
};
#endif /* COMMANDER_HPP_ */

4
src/modules/commander/commander_params.c
View File

@@ -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);