fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 07:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

Browse Source
This commit is contained in:
Daniel Agar
2019-08-04 10:08:09 -04:00
parent 839787568c
commit e69398c09f
62 changed files with 421 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+17 -19
View File
@@ -93,7 +93,6 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vtol_vehicle_status.h>
@@ -205,7 +204,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch
*/
void *commander_low_prio_loop(void *arg);
static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub);
static void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
{
@@ -249,7 +249,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
{
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = param1;
vcmd.param2 = param2;
@@ -262,9 +263,9 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2
vcmd.target_system = status.system_id;
vcmd.target_component = status.component_id;
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
return (h != nullptr);
return vcmd_pub.publish(vcmd);
}
int commander_main(int argc, char *argv[])
@@ -577,7 +578,7 @@ Commander::~Commander()
bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
orb_advert_t *command_ack_pub, bool *changed)
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -1078,13 +1079,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
default:
/* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
break;
}
if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(cmd, cmd_result, *command_ack_pub);
answer_command(cmd, cmd_result, command_ack_pub);
}
return true;
@@ -1239,7 +1240,7 @@ Commander::run()
hrt_abstime last_disarmed_timestamp = 0;
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
mission_init();
@@ -2152,7 +2153,7 @@ Commander::run()
cmd_sub.copy(&cmd);
/* handle it */
if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) {
status_changed = true;
}
}
@@ -3260,7 +3261,8 @@ print_reject_arm(const char *msg)
}
}
void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub)
void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub)
{
switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -3288,20 +3290,16 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
}
/* publish ACK */
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
command_ack_pub.publish(command_ack);
}
void *commander_low_prio_loop(void *arg)
@@ -3313,7 +3311,7 @@ void *commander_low_prio_loop(void *arg)
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
src/modules/commander/Commander.hpp
+5 -2
View File
@@ -45,8 +45,10 @@
// publications
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
@@ -180,7 +182,7 @@ private:
bool _flight_termination_triggered{false};
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
orb_advert_t *command_ack_pub, bool *changed);
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
bool set_home_position();
bool set_home_position_alt_only();
@@ -258,12 +260,13 @@ private:
// 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)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
};
#endif /* COMMANDER_HPP_ */
src/modules/commander/arm_auth.cpp
+4 -8
View File
@@ -39,10 +39,10 @@
#include <px4_config.h>
#include <systemlib/mavlink_log.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
static orb_advert_t handle_vehicle_command_pub;
static orb_advert_t *mavlink_log_pub;
static int command_ack_sub = -1;
@@ -78,17 +78,13 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
static void arm_auth_request_msg_send()
{
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
vcmd.target_system = arm_parameters.authorizer_system_id;
if (handle_vehicle_command_pub == nullptr) {
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
}
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
}
static uint8_t _auth_method_arm_req_check()
src/modules/commander/commander_helper.cpp
+1 -1
View File
@@ -290,7 +290,7 @@ int led_init()
led_control.mode = led_control_s::MODE_OFF;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);