dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Replace arm_request and mode_request with combined action_request 

Which saves flash space, log size and is extensible to handle e.g.
the VTOL transition and future actions.
This commit is contained in:
Matthias Grob 2021-10-19 15:54:49 +02:00
parent 052e29267d
commit 956997eb1e
8 changed files with 55 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/CMakeLists.txt
View File

@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW)
include(px4_list_make_absolute)
set(msg_files
action_request.msg
actuator_armed.msg
actuator_controls.msg
actuator_controls_status.msg
@ -47,7 +48,6 @@ set(msg_files
airspeed.msg
airspeed_validated.msg
airspeed_wind.msg
arm_request.msg
autotune_attitude_control_status.msg
battery_status.msg
camera_capture.msg
@ -109,7 +109,6 @@ set(msg_files
mavlink_log.msg
mission.msg
mission_result.msg
mode_request.msg
mount_orientation.msg
multirotor_motor_limits.msg
navigator_mission_item.msg

4
msg/arm_request.msg → msg/action_request.msg
View File

@ -6,8 +6,12 @@ uint8 ACTION_ARM = 1
uint8 ACTION_TOGGLE_ARMING = 2
uint8 ACTION_UNKILL = 3
uint8 ACTION_KILL = 4
uint8 ACTION_SWITCH_MODE = 5
uint8 source # how the request was triggered
uint8 SOURCE_RC_STICK_GESTURE = 0
uint8 SOURCE_RC_SWITCH = 1
uint8 SOURCE_RC_BUTTON = 2
uint8 SOURCE_RC_MODE_SLOT = 3
uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_

7
msg/mode_request.msg
View File

@ -1,7 +0,0 @@
uint64 timestamp # time since system start (microseconds)
uint8 mode # what mode is requested according to commander_state.MAIN_STATE_
uint8 source # how the request was triggered
uint8 SOURCE_RC_MODE_SLOT = 0
uint8 SOURCE_RC_SWITCH = 1

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

@ -2449,26 +2449,26 @@ Commander::run()
}
}
while (_arm_request_sub.updated()) {
arm_request_s arm_request;
while (_action_request_sub.updated()) {
action_request_s action_request;
if (_arm_request_sub.copy(&arm_request)) {
if (_action_request_sub.copy(&action_request)) {
arm_disarm_reason_t arm_disarm_reason{};
switch (arm_request.source) {
case arm_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
switch (action_request.source) {
case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
case arm_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
case arm_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break;
case action_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break;
}
switch (arm_request.action) {
case arm_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break;
switch (action_request.action) {
case action_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break;
case arm_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
case arm_request_s::ACTION_TOGGLE_ARMING:
case action_request_s::ACTION_TOGGLE_ARMING:
if (_armed.armed) {
disarm(arm_disarm_reason);
@ -2478,7 +2478,7 @@ Commander::run()
break;
case arm_request_s::ACTION_UNKILL:
case action_request_s::ACTION_UNKILL:
if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _armed.manual_lockdown) {
mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t");
events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged");
@ -2488,7 +2488,7 @@ Commander::run()
break;
case arm_request_s::ACTION_KILL:
case action_request_s::ACTION_KILL:
if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_armed.manual_lockdown) {
const char kill_switch_string[] = "Kill-switch engaged\t";
events::LogLevels log_levels{events::Log::Info};
@ -2508,18 +2508,14 @@ Commander::run()
}
break;
case action_request_s::ACTION_SWITCH_MODE:
main_state_transition(_status, action_request.mode, _status_flags, _internal_state);
break;
}
}
}
while (_mode_request_sub.updated()) {
mode_request_s mode_request;
if (_mode_request_sub.copy(&mode_request)) {
main_state_transition(_status, mode_request.mode, _status_flags, _internal_state);
}
}
/* Check for failure detector status */
if (_failure_detector.update(_status, _vehicle_control_mode)) {
_status.failure_detector_status = _failure_detector.getStatus().value;

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

@ -59,9 +59,9 @@
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/action_request.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/arm_request.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/distance_sensor.h>
@ -73,7 +73,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/mode_request.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
@ -390,8 +389,8 @@ private:
WorkerThread _worker_thread;
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
uORB::Subscription _arm_request_sub {ORB_ID(arm_request)};
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
@ -400,7 +399,6 @@ private:
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _mode_request_sub {ORB_ID(mode_request)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};

3
src/modules/logger/logged_topics.cpp
View File

@ -45,6 +45,7 @@ using namespace px4::logger;
void LoggedTopics::add_default_topics()
{
add_topic("action_request");
add_topic("actuator_armed");
add_topic("actuator_controls_0", 50);
add_topic("actuator_controls_1", 100);
@ -53,7 +54,6 @@ void LoggedTopics::add_default_topics()
add_topic("actuator_controls_status_0", 300);
add_topic("airspeed", 1000);
add_topic("airspeed_validated", 200);
add_topic("arm_request");
add_topic("autotune_attitude_control_status", 100);
add_topic("camera_capture");
add_topic("camera_trigger");
@ -73,7 +73,6 @@ void LoggedTopics::add_default_topics()
add_topic("manual_control_setpoint", 200);
add_topic("manual_control_switches");
add_topic("mission_result");
add_topic("mode_request");
add_topic("navigator_mission_item");
add_topic("offboard_control_mode", 100);
add_topic("onboard_computer_status", 10);

59
src/modules/manual_control/ManualControl.cpp
View File

@ -119,7 +119,7 @@ void ManualControl::Run()
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp);
if (!previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) {
sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_STICK_GESTURE);
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
// Disarm gesture
@ -130,7 +130,7 @@ void ManualControl::Run()
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp);
if (!previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_STICK_GESTURE);
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE);
}
// User override by stick
@ -165,24 +165,25 @@ void ManualControl::Run()
_button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now);
if (!previous_button_hysteresis && _button_hysteresis.get_state()) {
sendArmRequest(arm_request_s::ACTION_TOGGLE_ARMING, arm_request_s::SOURCE_RC_BUTTON);
sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_request_s::SOURCE_RC_BUTTON);
}
} else {
// Arming switch
if (switches.arm_switch != _previous_switches.arm_switch) {
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_SWITCH);
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_SWITCH);
}
}
}
if (switches.return_switch != _previous_switches.return_switch) {
if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendModeRequest(commander_state_s::MAIN_STATE_AUTO_RTL, mode_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
commander_state_s::MAIN_STATE_AUTO_RTL);
} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -191,7 +192,8 @@ void ManualControl::Run()
if (switches.loiter_switch != _previous_switches.loiter_switch) {
if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendModeRequest(commander_state_s::MAIN_STATE_AUTO_LOITER, mode_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
commander_state_s::MAIN_STATE_AUTO_LOITER);
} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -200,7 +202,8 @@ void ManualControl::Run()
if (switches.offboard_switch != _previous_switches.offboard_switch) {
if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendModeRequest(commander_state_s::MAIN_STATE_OFFBOARD, mode_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT,
commander_state_s::MAIN_STATE_OFFBOARD);
} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
evaluateModeSlot(switches.mode_slot);
@ -209,10 +212,10 @@ void ManualControl::Run()
if (switches.kill_switch != _previous_switches.kill_switch) {
if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
sendArmRequest(arm_request_s::ACTION_KILL, arm_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH);
} else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
sendArmRequest(arm_request_s::ACTION_UNKILL, arm_request_s::SOURCE_RC_SWITCH);
sendActionRequest(action_request_s::ACTION_UNKILL, action_request_s::SOURCE_RC_SWITCH);
}
}
@ -237,7 +240,7 @@ void ManualControl::Run()
}
} else {
// Send an initial command to switch to the mode requested by RC
// Send an initial request to switch to the mode requested by RC
evaluateModeSlot(switches.mode_slot);
}
@ -291,27 +294,27 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot)
break;
case manual_control_switches_s::MODE_SLOT_1:
sendModeRequest(_param_fltmode_1.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_1.get());
break;
case manual_control_switches_s::MODE_SLOT_2:
sendModeRequest(_param_fltmode_2.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_2.get());
break;
case manual_control_switches_s::MODE_SLOT_3:
sendModeRequest(_param_fltmode_3.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_3.get());
break;
case manual_control_switches_s::MODE_SLOT_4:
sendModeRequest(_param_fltmode_4.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_4.get());
break;
case manual_control_switches_s::MODE_SLOT_5:
sendModeRequest(_param_fltmode_5.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_5.get());
break;
case manual_control_switches_s::MODE_SLOT_6:
sendModeRequest(_param_fltmode_6.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_MODE_SLOT, _param_fltmode_6.get());
break;
default:
@ -320,22 +323,14 @@ void ManualControl::evaluateModeSlot(uint8_t mode_slot)
}
}
void ManualControl::sendModeRequest(uint8_t mode, uint8_t source)
void ManualControl::sendActionRequest(int8_t action, int8_t source, int8_t mode)
{
mode_request_s mode_request{};
mode_request.mode = mode;
mode_request.source = source;
mode_request.timestamp = hrt_absolute_time();
_mode_request_pub.publish(mode_request);
}
void ManualControl::sendArmRequest(int8_t action, int8_t source)
{
arm_request_s arm_request{};
arm_request.action = action;
arm_request.source = source;
arm_request.timestamp = hrt_absolute_time();
_arm_request_pub.publish(arm_request);
action_request_s action_request{};
action_request.action = action;
action_request.source = source;
action_request.mode = mode;
action_request.timestamp = hrt_absolute_time();
_action_request_pub.publish(action_request);
}
void ManualControl::publish_landing_gear(int8_t action)

9
src/modules/manual_control/ManualControl.hpp
View File

@ -40,11 +40,10 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/perf/perf_counter.h>
#include <uORB/topics/arm_request.h>
#include <uORB/topics/action_request.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mode_request.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
@ -121,14 +120,12 @@ private:
void Run() override;
void evaluateModeSlot(uint8_t mode_slot);
void sendModeRequest(uint8_t mode, uint8_t source);
void sendArmRequest(int8_t action, int8_t source);
void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);
void publish_landing_gear(int8_t action);
void send_vtol_transition_command(uint8_t action);
uORB::Publication<arm_request_s> _arm_request_pub{ORB_ID(arm_request)};
uORB::Publication<action_request_s> _action_request_pub{ORB_ID(action_request)};
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::Publication<mode_request_s> _mode_request_pub{ORB_ID(mode_request)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] {