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

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.
master
Matthias Grob 3 years ago
parent
052e29267d
commit
956997eb1e
8 changed files with 55 additions and 74 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. 3
      msg/CMakeLists.txt
  2. 4
      msg/action_request.msg
  3. 7
      msg/mode_request.msg
  4. 38
      src/modules/commander/Commander.cpp
  5. 6
      src/modules/commander/Commander.hpp
  6. 3
      src/modules/logger/logged_topics.cpp
  7. 59
      src/modules/manual_control/ManualControl.cpp
  8. 9
      src/modules/manual_control/ManualControl.hpp

3
msg/CMakeLists.txt
Unescape View File

@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW) @@ -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 @@ -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 @@ -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
Unescape View File

@ -6,8 +6,12 @@ uint8 ACTION_ARM = 1 @@ -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
Unescape View File

@ -1,7 +0,0 @@ @@ -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
Unescape View File

@ -2449,26 +2449,26 @@ Commander::run() @@ -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() @@ -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() @@ -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,15 +2508,11 @@ Commander::run() @@ -2508,15 +2508,11 @@ Commander::run()
}
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);
case action_request_s::ACTION_SWITCH_MODE:
main_state_transition(_status, action_request.mode, _status_flags, _internal_state);
break;
}
}
}

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

@ -59,9 +59,9 @@ @@ -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 @@ @@ -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: @@ -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: @@ -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
Unescape View File

@ -45,6 +45,7 @@ using namespace px4::logger; @@ -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() @@ -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() @@ -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
Unescape View File

@ -119,7 +119,7 @@ void ManualControl::Run() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) @@ -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) @@ -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
Unescape View File

@ -40,11 +40,10 @@ @@ -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: @@ -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] {

Write Preview
Loading…
Cancel
Save