mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 16:53:33 +08:00
Initialize all outgoing vehicle_command_ack_s and vehicle_command_s
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:
committed by
Lorenz Meier
parent
7c268f4fa1
commit
925efe990d
@@ -484,12 +484,11 @@ void
|
||||
CameraTrigger::test()
|
||||
{
|
||||
struct vehicle_command_s cmd = {};
|
||||
cmd.timestamp = hrt_absolute_time();
|
||||
cmd.timestamp = hrt_absolute_time(),
|
||||
cmd.param5 = 1.0f;
|
||||
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
|
||||
cmd.param5 = 1.0;
|
||||
|
||||
orb_advert_t pub;
|
||||
pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
(void)orb_unadvertise(pub);
|
||||
}
|
||||
|
||||
@@ -509,7 +508,7 @@ CameraTrigger::cycle_trampoline(void *arg)
|
||||
bool updated = false;
|
||||
orb_check(trig->_command_sub, &updated);
|
||||
|
||||
struct vehicle_command_s cmd = {};
|
||||
struct vehicle_command_s cmd;
|
||||
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
bool need_ack = false;
|
||||
|
||||
@@ -716,10 +715,11 @@ CameraTrigger::cycle_trampoline(void *arg)
|
||||
|
||||
// Command ACK handling
|
||||
if (updated && need_ack) {
|
||||
vehicle_command_ack_s command_ack = {};
|
||||
|
||||
command_ack.command = cmd.command;
|
||||
command_ack.result = cmd_result;
|
||||
vehicle_command_ack_s command_ack = {
|
||||
.timestamp = 0,
|
||||
.command = cmd.command,
|
||||
.result = (uint8_t)cmd_result
|
||||
};
|
||||
|
||||
if (trig->_cmd_ack_pub == nullptr) {
|
||||
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||
|
||||
+19
-19
@@ -792,8 +792,6 @@ PX4IO::init()
|
||||
|
||||
} while (true);
|
||||
|
||||
/* send command to arm system via command API */
|
||||
vehicle_command_s cmd;
|
||||
/* send this to itself */
|
||||
param_t sys_id_param = param_find("MAV_SYS_ID");
|
||||
param_t comp_id_param = param_find("MAV_COMP_ID");
|
||||
@@ -809,23 +807,25 @@ PX4IO::init()
|
||||
errx(1, "PRM CMPID");
|
||||
}
|
||||
|
||||
cmd.target_system = sys_id;
|
||||
cmd.target_component = comp_id;
|
||||
cmd.source_system = sys_id;
|
||||
cmd.source_component = comp_id;
|
||||
/* request arming */
|
||||
cmd.param1 = 1.0f;
|
||||
cmd.param2 = 0;
|
||||
cmd.param3 = 0;
|
||||
cmd.param4 = 0;
|
||||
cmd.param5 = 0;
|
||||
cmd.param6 = 0;
|
||||
cmd.param7 = 0;
|
||||
cmd.timestamp = hrt_absolute_time();
|
||||
cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
|
||||
|
||||
/* ask to confirm command */
|
||||
cmd.confirmation = 1;
|
||||
/* send command to arm system via command API */
|
||||
struct vehicle_command_s cmd = {
|
||||
.timestamp = hrt_absolute_time(),
|
||||
.param5 = 0.0f,
|
||||
.param6 = 0.0f,
|
||||
/* request arming */
|
||||
.param1 = 1.0f,
|
||||
.param2 = 0.0f,
|
||||
.param3 = 0.0f,
|
||||
.param4 = 0.0f,
|
||||
.param7 = 0.0f,
|
||||
.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
|
||||
.target_system = (uint8_t)sys_id,
|
||||
.target_component = (uint8_t)comp_id,
|
||||
.source_system = (uint8_t)sys_id,
|
||||
.source_component = (uint8_t)comp_id,
|
||||
/* ask to confirm command */
|
||||
.confirmation = 1
|
||||
};
|
||||
|
||||
/* send command once */
|
||||
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
|
||||
@@ -285,10 +285,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
|
||||
|
||||
void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
|
||||
{
|
||||
vehicle_command_ack_s vehicle_command_ack;
|
||||
vehicle_command_ack.timestamp = hrt_absolute_time();
|
||||
vehicle_command_ack.command = command;
|
||||
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
vehicle_command_ack_s vehicle_command_ack = {
|
||||
.timestamp = hrt_absolute_time(),
|
||||
.command = command,
|
||||
.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED
|
||||
};
|
||||
|
||||
if (_vehicle_command_ack_pub == nullptr) {
|
||||
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
||||
|
||||
@@ -54,15 +54,25 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config)
|
||||
|
||||
int OutputMavlink::update(const ControlData *control_data)
|
||||
{
|
||||
vehicle_command_s vehicle_command;
|
||||
vehicle_command_s vehicle_command = {
|
||||
.timestamp = 0,
|
||||
.param5 = 0.0f,
|
||||
.param6 = 0.0f,
|
||||
.param1 = 0.0f,
|
||||
.param2 = 0.0f,
|
||||
.param3 = 0.0f,
|
||||
.param4 = 0.0f,
|
||||
.param7 = 0.0f,
|
||||
.command = 0,
|
||||
.target_system = (uint8_t)_config.mavlink_sys_id,
|
||||
.target_component = (uint8_t)_config.mavlink_comp_id,
|
||||
};
|
||||
|
||||
if (control_data) {
|
||||
//got new command
|
||||
_set_angle_setpoints(control_data);
|
||||
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
|
||||
vehicle_command.target_system = _config.mavlink_sys_id;
|
||||
vehicle_command.target_component = _config.mavlink_comp_id;
|
||||
vehicle_command.timestamp = hrt_absolute_time();
|
||||
|
||||
if (control_data->type == ControlData::Type::Neutral) {
|
||||
@@ -93,8 +103,6 @@ int OutputMavlink::update(const ControlData *control_data)
|
||||
|
||||
vehicle_command.timestamp = t;
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
vehicle_command.target_system = _config.mavlink_sys_id;
|
||||
vehicle_command.target_component = _config.mavlink_comp_id;
|
||||
|
||||
vehicle_command.param1 = _angle_outputs[0];
|
||||
vehicle_command.param2 = _angle_outputs[1];
|
||||
|
||||
Reference in New Issue
Block a user