mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 17:19:06 +08:00
commander move most orb subscriptions to uORB::Subscription
This commit is contained in:
Daniel Agar
parent
777b615cf9
commit
3faab909d7
@ -558,11 +558,6 @@ Commander::Commander() :
|
||||
{
|
||||
_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
|
||||
_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
|
||||
_battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
|
||||
|
||||
_telemetry_status_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
|
||||
|
||||
// We want to accept RC inputs as default
|
||||
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
|
||||
@ -582,20 +577,10 @@ Commander::Commander() :
|
||||
status_flags.rc_calibration_valid = true;
|
||||
|
||||
status_flags.avoidance_system_valid = false;
|
||||
|
||||
|
||||
}
|
||||
|
||||
Commander::~Commander()
|
||||
{
|
||||
orb_unsubscribe(_battery_sub);
|
||||
orb_unsubscribe(_telemetry_status_sub);
|
||||
|
||||
|
||||
if (_iridiumsbd_status_sub >= 0) {
|
||||
orb_unsubscribe(_iridiumsbd_status_sub);
|
||||
}
|
||||
|
||||
delete[] _airspeed_fault_type;
|
||||
}
|
||||
|
||||
@ -1230,7 +1215,7 @@ Commander::run()
|
||||
PX4_WARN("Buzzer init failed");
|
||||
}
|
||||
|
||||
int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state));
|
||||
uORB::Subscription power_button_state_sub{ORB_ID(power_button_state)};
|
||||
{
|
||||
// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
|
||||
// in IRQ context.
|
||||
@ -1238,7 +1223,8 @@ Commander::run()
|
||||
button_state.timestamp = 0;
|
||||
button_state.event = 0xff;
|
||||
power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
|
||||
orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state);
|
||||
|
||||
power_button_state_sub.copy(&button_state);
|
||||
}
|
||||
|
||||
if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
|
||||
@ -1275,18 +1261,18 @@ Commander::run()
|
||||
|
||||
bool updated = false;
|
||||
|
||||
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
|
||||
int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
|
||||
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
|
||||
int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
|
||||
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
int safety_sub = orb_subscribe(ORB_ID(safety));
|
||||
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
|
||||
int system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
|
||||
uORB::Subscription actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
|
||||
uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
|
||||
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)};
|
||||
uORB::Subscription subsys_sub{ORB_ID(subsystem_info)};
|
||||
uORB::Subscription system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
|
||||
geofence_result_s geofence_result {};
|
||||
|
||||
@ -1388,14 +1374,13 @@ Commander::run()
|
||||
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
|
||||
|
||||
/* update parameters */
|
||||
bool params_updated = false;
|
||||
orb_check(param_changed_sub, ¶ms_updated);
|
||||
bool params_updated = param_changed_sub.updated();
|
||||
|
||||
if (params_updated || param_init_forced) {
|
||||
|
||||
/* parameters changed */
|
||||
struct parameter_update_s param_changed;
|
||||
orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed);
|
||||
parameter_update_s param_changed;
|
||||
param_changed_sub.copy(¶m_changed);
|
||||
|
||||
updateParams();
|
||||
|
||||
@ -1490,28 +1475,20 @@ Commander::run()
|
||||
status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
|
||||
/* handle power button state */
|
||||
orb_check(power_button_state_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (power_button_state_sub.updated()) {
|
||||
power_button_state_s button_state;
|
||||
orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state);
|
||||
power_button_state_sub.copy(&button_state);
|
||||
|
||||
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
|
||||
px4_shutdown_request(false, false);
|
||||
}
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
sp_man_sub.update(&sp_man);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
|
||||
}
|
||||
|
||||
orb_check(offboard_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (offboard_control_mode_sub.updated()) {
|
||||
offboard_control_mode_s old = offboard_control_mode;
|
||||
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &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 ||
|
||||
@ -1558,11 +1535,10 @@ Commander::run()
|
||||
}
|
||||
}
|
||||
|
||||
orb_check(system_power_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (system_power_sub.updated()) {
|
||||
system_power_s system_power = {};
|
||||
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
|
||||
system_power_sub.copy(&system_power);
|
||||
|
||||
if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
|
||||
if (system_power.servo_valid &&
|
||||
@ -1589,12 +1565,10 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* update safety topic */
|
||||
orb_check(safety_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (safety_sub.updated()) {
|
||||
bool previous_safety_off = safety.safety_off;
|
||||
|
||||
if (orb_copy(ORB_ID(safety), safety_sub, &safety) == PX4_OK) {
|
||||
if (safety_sub.copy(&safety)) {
|
||||
|
||||
/* disarm if safety is now on and still armed */
|
||||
if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF)
|
||||
@ -1624,11 +1598,9 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* update vtol vehicle status*/
|
||||
orb_check(vtol_vehicle_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (vtol_vehicle_status_sub.updated()) {
|
||||
/* vtol status changed */
|
||||
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
|
||||
vtol_vehicle_status_sub.copy(&vtol_status);
|
||||
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
|
||||
|
||||
/* Make sure that this is only adjusted if vehicle really is of type vtol */
|
||||
@ -1666,10 +1638,8 @@ Commander::run()
|
||||
airspeed_use_check();
|
||||
|
||||
/* Update land detector */
|
||||
orb_check(land_detector_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
|
||||
if (land_detector_sub.updated()) {
|
||||
land_detector_sub.copy(&land_detector);
|
||||
|
||||
// Only take actions if armed
|
||||
if (armed.armed) {
|
||||
@ -1749,21 +1719,15 @@ Commander::run()
|
||||
_geofence_warning_action_on = false;
|
||||
}
|
||||
|
||||
orb_check(cpuload_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
|
||||
}
|
||||
cpuload_sub.update(&cpuload);
|
||||
|
||||
battery_status_check();
|
||||
|
||||
/* update subsystem info which arrives from outside of commander*/
|
||||
do {
|
||||
orb_check(subsys_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (subsys_sub.updated()) {
|
||||
subsystem_info_s info{};
|
||||
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
|
||||
subsys_sub.copy(&info);
|
||||
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
|
||||
status_changed = true;
|
||||
}
|
||||
@ -1826,11 +1790,7 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* start geofence result check */
|
||||
orb_check(geofence_result_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
|
||||
}
|
||||
geofence_result_sub.update(&geofence_result);
|
||||
|
||||
// Geofence actions
|
||||
const bool geofence_action_enabled = geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
|
||||
@ -2158,9 +2118,7 @@ Commander::run()
|
||||
|
||||
// engine failure detection
|
||||
// TODO: move out of commander
|
||||
orb_check(actuator_controls_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (actuator_controls_sub.updated()) {
|
||||
/* Check engine failure
|
||||
* only for fixed wing for now
|
||||
*/
|
||||
@ -2168,7 +2126,7 @@ Commander::run()
|
||||
!status.is_rotary_wing && !status.is_vtol && armed.armed) {
|
||||
|
||||
actuator_controls_s actuator_controls = {};
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
actuator_controls_sub.copy(&actuator_controls);
|
||||
|
||||
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
|
||||
const float current2throttle = _battery_current / throttle;
|
||||
@ -2235,13 +2193,11 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* handle commands last, as the system needs to be updated to handle them */
|
||||
orb_check(cmd_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
struct vehicle_command_s cmd;
|
||||
if (cmd_sub.updated()) {
|
||||
vehicle_command_s cmd{};
|
||||
|
||||
/* got command */
|
||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
cmd_sub.copy(&cmd);
|
||||
|
||||
/* handle it */
|
||||
if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
|
||||
@ -2498,13 +2454,6 @@ Commander::run()
|
||||
/* close fds */
|
||||
led_deinit();
|
||||
buzzer_deinit();
|
||||
orb_unsubscribe(sp_man_sub);
|
||||
orb_unsubscribe(offboard_control_mode_sub);
|
||||
orb_unsubscribe(safety_sub);
|
||||
orb_unsubscribe(cmd_sub);
|
||||
orb_unsubscribe(subsys_sub);
|
||||
orb_unsubscribe(param_changed_sub);
|
||||
orb_unsubscribe(land_detector_sub);
|
||||
|
||||
thread_running = false;
|
||||
}
|
||||
@ -3500,7 +3449,7 @@ void *commander_low_prio_loop(void *arg)
|
||||
continue;
|
||||
|
||||
} else if (pret != 0) {
|
||||
struct vehicle_command_s cmd;
|
||||
struct vehicle_command_s cmd {};
|
||||
|
||||
/* if we reach here, we have a valid command */
|
||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
@ -3826,15 +3775,11 @@ bool Commander::preflight_check(bool report)
|
||||
|
||||
void Commander::data_link_check(bool &status_changed)
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
orb_check(_telemetry_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
if (_telemetry_status_sub.updated()) {
|
||||
|
||||
telemetry_status_s telemetry;
|
||||
|
||||
if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub, &telemetry) == PX4_OK) {
|
||||
if (_telemetry_status_sub.copy(&telemetry)) {
|
||||
|
||||
// handle different radio types
|
||||
switch (telemetry.type) {
|
||||
@ -3843,38 +3788,25 @@ void Commander::data_link_check(bool &status_changed)
|
||||
status_flags.usb_connected = true;
|
||||
break;
|
||||
|
||||
case telemetry_status_s::LINK_TYPE_IRIDIUM:
|
||||
case telemetry_status_s::LINK_TYPE_IRIDIUM: {
|
||||
|
||||
// lazily subscribe
|
||||
if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
|
||||
_iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
|
||||
}
|
||||
iridiumsbd_status_s iridium_status;
|
||||
|
||||
if (_iridiumsbd_status_sub >= 0) {
|
||||
bool iridiumsbd_updated = false;
|
||||
orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
|
||||
if (_iridiumsbd_status_sub.update(&iridium_status)) {
|
||||
_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
|
||||
|
||||
if (iridiumsbd_updated) {
|
||||
iridiumsbd_status_s iridium_status;
|
||||
|
||||
if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
|
||||
_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
|
||||
|
||||
if (status.high_latency_data_link_lost) {
|
||||
if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
|
||||
status.high_latency_data_link_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
if (status.high_latency_data_link_lost) {
|
||||
if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
|
||||
status.high_latency_data_link_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
// handle different remote types
|
||||
switch (telemetry.remote_type) {
|
||||
case telemetry_status_s::MAV_TYPE_GCS:
|
||||
@ -4021,16 +3953,11 @@ void Commander::data_link_check(bool &status_changed)
|
||||
|
||||
void Commander::battery_status_check()
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
/* update battery status */
|
||||
orb_check(_battery_sub, &updated);
|
||||
if (_battery_sub.updated()) {
|
||||
battery_status_s battery{};
|
||||
|
||||
if (updated) {
|
||||
|
||||
battery_status_s battery = {};
|
||||
|
||||
if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) {
|
||||
if (_battery_sub.copy(&battery)) {
|
||||
|
||||
if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
|
||||
&& battery.connected
|
||||
|
||||
@ -208,7 +208,7 @@ private:
|
||||
*/
|
||||
void data_link_check(bool &status_changed);
|
||||
|
||||
int _telemetry_status_sub{-1};
|
||||
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
|
||||
|
||||
hrt_abstime _datalink_last_heartbeat_gcs{0};
|
||||
|
||||
@ -221,12 +221,12 @@ private:
|
||||
bool _avoidance_system_status_change{false};
|
||||
uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
|
||||
|
||||
int _iridiumsbd_status_sub{-1};
|
||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
|
||||
hrt_abstime _high_latency_datalink_heartbeat{0};
|
||||
hrt_abstime _high_latency_datalink_lost{0};
|
||||
|
||||
int _battery_sub{-1};
|
||||
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
float _battery_current{0.0f};
|
||||
|
||||
|
||||
@ -148,6 +148,7 @@
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/sensor_correction.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
static const char *sensor_name = "accel";
|
||||
|
||||
@ -334,11 +335,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
||||
|
||||
if (tc_enabled_int == 1) {
|
||||
/* Get struct containing sensor thermal compensation data */
|
||||
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
|
||||
memset(&sensor_correction, 0, sizeof(sensor_correction));
|
||||
int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
|
||||
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
|
||||
orb_unsubscribe(sensor_correction_sub);
|
||||
sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
|
||||
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
|
||||
sensor_correction_sub.copy(&sensor_correction);
|
||||
|
||||
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
|
||||
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user