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

vehicle_command topic: use uorb queuing with length 3

Browse Source 
Just to make sure we don't lose any messages.
This commit is contained in:
Beat Küng
2016-09-07 18:47:12 +02:00
parent c1b73a068e
commit be4db3c5df
9 changed files with 29 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_command.msg
+2
View File
@@ -83,6 +83,8 @@ uint32 VEHICLE_ROI_LOCATION = 3 # Point toward fixed locatio
uint32 VEHICLE_ROI_TARGET = 4 # Point toward target uint32 VEHICLE_ROI_TARGET = 4 # Point toward target
uint32 VEHICLE_ROI_ENUM_END = 5 uint32 VEHICLE_ROI_ENUM_END = 5
uint32 ORB_QUEUE_LENGTH = 3
float32 param1 # Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum. float32 param1 # Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum.
float32 param2 # Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum. float32 param2 # Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.
float32 param3 # Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum. float32 param3 # Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.
msg/vehicle_command_ack.msg
+2
View File
@@ -4,5 +4,7 @@ uint8 VEHICLE_RESULT_DENIED = 2
uint8 VEHICLE_RESULT_UNSUPPORTED = 3 uint8 VEHICLE_RESULT_UNSUPPORTED = 3
uint8 VEHICLE_RESULT_FAILED = 4 uint8 VEHICLE_RESULT_FAILED = 4
uint32 ORB_QUEUE_LENGTH = 3
uint16 command uint16 command
uint8 result uint8 result
src/drivers/camera_trigger/camera_trigger.cpp
+1 -1
View File
@@ -395,7 +395,7 @@ CameraTrigger::test()
cmd.param5 = 1.0f; cmd.param5 = 1.0f;
orb_advert_t pub; orb_advert_t pub;
pub = orb_advertise(ORB_ID(vehicle_command), &cmd); pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(pub); (void)orb_unadvertise(pub);
} }
src/drivers/px4io/px4io.cpp
+1 -1
View File
@@ -825,7 +825,7 @@ PX4IO::init()
cmd.confirmation = 1; cmd.confirmation = 1;
/* send command once */ /* send command once */
orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
/* spin here until IO's state has propagated into the system */ /* spin here until IO's state has propagated into the system */
do { do {
src/drivers/vmount/input_mavlink.cpp
+8 -3
View File
@@ -302,9 +302,14 @@ void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
vehicle_command_ack.command = command; vehicle_command_ack.command = command;
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
int instance; if (_vehicle_command_ack_pub == nullptr) {
orb_publish_auto(ORB_ID(vehicle_command_ack), &_vehicle_command_ack_pub, &vehicle_command_ack, _vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
&instance, ORB_PRIO_DEFAULT); vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
}
} }
void InputMavlinkCmdMount::print_status() void InputMavlinkCmdMount::print_status()
src/drivers/vmount/output_mavlink.cpp
+2 -1
View File
@@ -75,7 +75,8 @@ int OutputMavlink::update(const ControlData *control_data)
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
} else { } else {
_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, 3); _vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} }
} }
src/modules/commander/commander.cpp
+6 -6
View File
@@ -424,7 +424,7 @@ int commander_main(int argc, char *argv[])
cmd.param6 = NAN; cmd.param6 = NAN;
cmd.param7 = NAN; cmd.param7 = NAN;
orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h); (void)orb_unadvertise(h);
} else { } else {
@@ -453,7 +453,7 @@ int commander_main(int argc, char *argv[])
cmd.param6 = NAN; cmd.param6 = NAN;
cmd.param7 = NAN; cmd.param7 = NAN;
orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h); (void)orb_unadvertise(h);
return 0; return 0;
@@ -476,7 +476,7 @@ int commander_main(int argc, char *argv[])
cmd.param6 = NAN; cmd.param6 = NAN;
cmd.param7 = NAN; cmd.param7 = NAN;
orb_advert_t h = orb_advertise(ORB_ID(vehicle_command), &cmd); orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h); (void)orb_unadvertise(h);
return 0; return 0;
@@ -538,8 +538,8 @@ int commander_main(int argc, char *argv[])
/* if the comparison matches for off (== 0) set 0.0f, 2.0f (on) else */ /* if the comparison matches for off (== 0) set 0.0f, 2.0f (on) else */
cmd.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f; /* lockdown */ cmd.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f; /* lockdown */
// XXX inspect use of publication handle orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_advertise(ORB_ID(vehicle_command), &cmd); (void)orb_unadvertise(h);
return 0; return 0;
} }
@@ -3665,7 +3665,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else { } else {
command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &command_ack); command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} }
} }
src/modules/mavlink/mavlink_receiver.cpp
+3 -3
View File
@@ -412,7 +412,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.confirmation = cmd_mavlink.confirmation; vcmd.confirmation = cmd_mavlink.confirmation;
if (_cmd_pub == nullptr) { if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else { } else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
@@ -488,7 +488,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
if (_cmd_pub == nullptr) { if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else { } else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
@@ -636,7 +636,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.confirmation = 1; vcmd.confirmation = 1;
if (_cmd_pub == nullptr) { if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else { } else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
src/modules/navigator/mission_block.cpp
+4 -4
View File
@@ -364,7 +364,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
} }
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) { if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_WARN("do_set_servo command"); PX4_INFO("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else // XXX: we should issue a vehicle command and handle this somewhere else
memset(&actuators, 0, sizeof(actuators)); memset(&actuators, 0, sizeof(actuators));
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6) // params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
@@ -380,7 +380,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
} }
} else { } else {
PX4_WARN("forwarding command %d\n", item->nav_cmd); PX4_INFO("forwarding command %d", item->nav_cmd);
struct vehicle_command_s cmd = {}; struct vehicle_command_s cmd = {};
mission_item_to_vehicle_command(item, &cmd); mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time(); _action_start = hrt_absolute_time();
@@ -389,7 +389,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else { } else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} }
} }
} }
@@ -616,7 +616,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
if (_cmd_pub != nullptr) { if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else { } else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} }
} }