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

Initialize all outgoing vehicle_command_ack_s and vehicle_command_s

Browse Source 
This will initialize those structs with zero in all fields not set
and all fields set will only be change once to the final value not
wasting CPU time zeroing it.

This will guarantee that no non-unitialized structs will have
a trash value on from_external causing it to be sent to the
MAVLink channel without need it.
This commit is contained in:
José Roberto de Souza
2017-08-08 21:46:30 +02:00
committed by Lorenz Meier
parent 7c268f4fa1
commit 925efe990d
17 changed files with 267 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -2200,7 +2200,7 @@ Mavlink::task_main(int argc, char *argv[])
/* send command ACK */
uint16_t current_command_ack = 0;
struct vehicle_command_ack_s command_ack = {};
struct vehicle_command_ack_s command_ack;
if (ack_sub->update(&ack_time, &command_ack)) {
if (!command_ack.from_external) {
src/modules/mavlink/mavlink_messages.cpp
+13 -13
View File
@@ -1410,19 +1410,19 @@ protected:
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
vehicle_command_s cmd{};
cmd.target_system = mavlink_system.sysid;
cmd.target_component = MAV_COMP_ID_CAMERA;
cmd.command = MAV_CMD_IMAGE_START_CAPTURE;
cmd.confirmation = 0;
cmd.param1 = 0; // all cameras
cmd.param2 = 0; // duration 0 because only taking one picture
cmd.param3 = 1; // only take one
cmd.param4 = NAN;
cmd.param5 = NAN;
cmd.param6 = NAN;
cmd.param7 = NAN;
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = NAN,
.param6 = NAN,
.param1 = 0.0f, // all cameras
.param2 = 0.0f, // duration 0 because only taking one picture
.param3 = 1.0f, // only take one
.param4 = NAN,
.param7 = NAN,
.command = MAV_CMD_IMAGE_START_CAPTURE,
.target_system = mavlink_system.sysid,
.target_component = MAV_COMP_ID_CAMERA
};
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
src/modules/mavlink/mavlink_receiver.cpp
+74 -111
View File
@@ -439,41 +439,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->request_stop_ulog_streaming();
}
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
vcmd.timestamp = hrt_absolute_time();
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
// XXX do proper translation
vcmd.command = cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
vcmd.from_external = 1;
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = cmd_mavlink.param5,
.param6 = cmd_mavlink.param6,
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
.param1 = cmd_mavlink.param1,
.param2 = cmd_mavlink.param2,
.param3 = cmd_mavlink.param3,
.param4 = cmd_mavlink.param4,
.param7 = cmd_mavlink.param7,
// XXX do proper translation
.command = cmd_mavlink.command,
.target_system = cmd_mavlink.target_system,
.target_component = cmd_mavlink.target_component,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = cmd_mavlink.confirmation,
.from_external = 1
};
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -486,15 +470,11 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
out:
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;
if (ret == PX4_OK) {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
};
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -550,40 +530,26 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
send_ack = false;
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
vcmd.timestamp = hrt_absolute_time();
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z;
// XXX do proper translation
vcmd.command = cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.from_external = 1;
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
.param5 = ((double)cmd_mavlink.x) / 1e7,
.param6 = ((double)cmd_mavlink.y) / 1e7,
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
.param1 = cmd_mavlink.param1,
.param2 = cmd_mavlink.param2,
.param3 = cmd_mavlink.param3,
.param4 = cmd_mavlink.param4,
.param7 = cmd_mavlink.z,
// XXX do proper translation
.command = cmd_mavlink.command,
.target_system = cmd_mavlink.target_system,
.target_component = cmd_mavlink.target_component,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = 0,
.from_external = 1
};
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -596,15 +562,11 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
out:
if (send_ack) {
vehicle_command_ack_s command_ack;
command_ack.command = cmd_mavlink.command;
if (ret == PX4_OK) {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
};
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -624,11 +586,12 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid);
vehicle_command_ack_s command_ack = {};
command_ack.command = ack.command;
command_ack.result = ack.result;
command_ack.timestamp = hrt_absolute_time();
command_ack.from_external = 1;
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.command = ack.command,
.result = ack.result,
.from_external = 1
};
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -764,27 +727,27 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd));
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = custom_mode.sub_mode;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
vcmd.timestamp = hrt_absolute_time();
vcmd.from_external = 1;
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = 0,
.param6 = 0,
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
.param1 = (float)new_mode.base_mode,
.param2 = (float)custom_mode.main_mode,
.param3 = (float)custom_mode.sub_mode,
.param4 = 0,
.param7 = 0,
.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE,
.target_system = new_mode.target_system,
.target_component = MAV_COMP_ID_ALL,
.source_system = msg->sysid,
.source_component = msg->compid,
.confirmation = 1,
.from_external = 1
};
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);