mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:38:52 +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()
|
CameraTrigger::test()
|
||||||
{
|
{
|
||||||
struct vehicle_command_s cmd = {};
|
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.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
|
||||||
cmd.param5 = 1.0;
|
|
||||||
|
|
||||||
orb_advert_t pub;
|
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
|
||||||
(void)orb_unadvertise(pub);
|
(void)orb_unadvertise(pub);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -509,7 +508,7 @@ CameraTrigger::cycle_trampoline(void *arg)
|
|||||||
bool updated = false;
|
bool updated = false;
|
||||||
orb_check(trig->_command_sub, &updated);
|
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;
|
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||||
bool need_ack = false;
|
bool need_ack = false;
|
||||||
|
|
||||||
@@ -716,10 +715,11 @@ CameraTrigger::cycle_trampoline(void *arg)
|
|||||||
|
|
||||||
// Command ACK handling
|
// Command ACK handling
|
||||||
if (updated && need_ack) {
|
if (updated && need_ack) {
|
||||||
vehicle_command_ack_s command_ack = {};
|
vehicle_command_ack_s command_ack = {
|
||||||
|
.timestamp = 0,
|
||||||
command_ack.command = cmd.command;
|
.command = cmd.command,
|
||||||
command_ack.result = cmd_result;
|
.result = (uint8_t)cmd_result
|
||||||
|
};
|
||||||
|
|
||||||
if (trig->_cmd_ack_pub == nullptr) {
|
if (trig->_cmd_ack_pub == nullptr) {
|
||||||
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
||||||
|
|||||||
+19
-19
@@ -792,8 +792,6 @@ PX4IO::init()
|
|||||||
|
|
||||||
} while (true);
|
} while (true);
|
||||||
|
|
||||||
/* send command to arm system via command API */
|
|
||||||
vehicle_command_s cmd;
|
|
||||||
/* send this to itself */
|
/* send this to itself */
|
||||||
param_t sys_id_param = param_find("MAV_SYS_ID");
|
param_t sys_id_param = param_find("MAV_SYS_ID");
|
||||||
param_t comp_id_param = param_find("MAV_COMP_ID");
|
param_t comp_id_param = param_find("MAV_COMP_ID");
|
||||||
@@ -809,23 +807,25 @@ PX4IO::init()
|
|||||||
errx(1, "PRM CMPID");
|
errx(1, "PRM CMPID");
|
||||||
}
|
}
|
||||||
|
|
||||||
cmd.target_system = sys_id;
|
/* send command to arm system via command API */
|
||||||
cmd.target_component = comp_id;
|
struct vehicle_command_s cmd = {
|
||||||
cmd.source_system = sys_id;
|
.timestamp = hrt_absolute_time(),
|
||||||
cmd.source_component = comp_id;
|
.param5 = 0.0f,
|
||||||
/* request arming */
|
.param6 = 0.0f,
|
||||||
cmd.param1 = 1.0f;
|
/* request arming */
|
||||||
cmd.param2 = 0;
|
.param1 = 1.0f,
|
||||||
cmd.param3 = 0;
|
.param2 = 0.0f,
|
||||||
cmd.param4 = 0;
|
.param3 = 0.0f,
|
||||||
cmd.param5 = 0;
|
.param4 = 0.0f,
|
||||||
cmd.param6 = 0;
|
.param7 = 0.0f,
|
||||||
cmd.param7 = 0;
|
.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
|
||||||
cmd.timestamp = hrt_absolute_time();
|
.target_system = (uint8_t)sys_id,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
|
.target_component = (uint8_t)comp_id,
|
||||||
|
.source_system = (uint8_t)sys_id,
|
||||||
/* ask to confirm command */
|
.source_component = (uint8_t)comp_id,
|
||||||
cmd.confirmation = 1;
|
/* ask to confirm command */
|
||||||
|
.confirmation = 1
|
||||||
|
};
|
||||||
|
|
||||||
/* send command once */
|
/* send command once */
|
||||||
orb_advert_t 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);
|
||||||
|
|||||||
@@ -285,10 +285,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
|
|||||||
|
|
||||||
void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
|
void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
|
||||||
{
|
{
|
||||||
vehicle_command_ack_s vehicle_command_ack;
|
vehicle_command_ack_s vehicle_command_ack = {
|
||||||
vehicle_command_ack.timestamp = hrt_absolute_time();
|
.timestamp = hrt_absolute_time(),
|
||||||
vehicle_command_ack.command = command;
|
.command = command,
|
||||||
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED
|
||||||
|
};
|
||||||
|
|
||||||
if (_vehicle_command_ack_pub == nullptr) {
|
if (_vehicle_command_ack_pub == nullptr) {
|
||||||
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
_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)
|
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) {
|
if (control_data) {
|
||||||
//got new command
|
//got new command
|
||||||
_set_angle_setpoints(control_data);
|
_set_angle_setpoints(control_data);
|
||||||
|
|
||||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
|
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();
|
vehicle_command.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
if (control_data->type == ControlData::Type::Neutral) {
|
if (control_data->type == ControlData::Type::Neutral) {
|
||||||
@@ -93,8 +103,6 @@ int OutputMavlink::update(const ControlData *control_data)
|
|||||||
|
|
||||||
vehicle_command.timestamp = t;
|
vehicle_command.timestamp = t;
|
||||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
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.param1 = _angle_outputs[0];
|
||||||
vehicle_command.param2 = _angle_outputs[1];
|
vehicle_command.param2 = _angle_outputs[1];
|
||||||
|
|||||||
@@ -836,7 +836,6 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
|
|||||||
|
|
||||||
if (px4_poll(&fds[0], 1, 0) > 0) {
|
if (px4_poll(&fds[0], 1, 0) > 0) {
|
||||||
struct vehicle_command_s cmd;
|
struct vehicle_command_s cmd;
|
||||||
memset(&cmd, 0, sizeof(cmd));
|
|
||||||
|
|
||||||
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
|
||||||
|
|
||||||
|
|||||||
@@ -498,21 +498,20 @@ int commander_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
|
||||||
|
|
||||||
vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.target_system = status.system_id;
|
.timestamp = hrt_absolute_time(),
|
||||||
cmd.target_component = status.component_id;
|
.param5 = NAN,
|
||||||
|
.param6 = NAN,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
|
/* minimum pitch */
|
||||||
cmd.param1 = NAN; /* minimum pitch */
|
.param1 = NAN,
|
||||||
/* param 2-3 unused */
|
.param2 = NAN,
|
||||||
cmd.param2 = NAN;
|
.param3 = NAN,
|
||||||
cmd.param3 = NAN;
|
.param4 = NAN,
|
||||||
cmd.param4 = NAN;
|
.param7 = NAN,
|
||||||
cmd.param5 = NAN;
|
.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
|
||||||
cmd.param6 = NAN;
|
.target_system = (uint8_t)status.system_id,
|
||||||
cmd.param7 = NAN;
|
.target_component = (uint8_t)status.component_id
|
||||||
|
};
|
||||||
cmd.timestamp = hrt_absolute_time();
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
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);
|
||||||
@@ -530,18 +529,20 @@ int commander_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (!strcmp(argv[1], "land")) {
|
if (!strcmp(argv[1], "land")) {
|
||||||
|
|
||||||
vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.target_system = status.system_id;
|
.timestamp = 0,
|
||||||
cmd.target_component = status.component_id;
|
.param5 = NAN,
|
||||||
|
.param6 = NAN,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
|
/* minimum pitch */
|
||||||
/* param 2-3 unused */
|
.param1 = NAN,
|
||||||
cmd.param2 = NAN;
|
.param2 = NAN,
|
||||||
cmd.param3 = NAN;
|
.param3 = NAN,
|
||||||
cmd.param4 = NAN;
|
.param4 = NAN,
|
||||||
cmd.param5 = NAN;
|
.param7 = NAN,
|
||||||
cmd.param6 = NAN;
|
.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND,
|
||||||
cmd.param7 = NAN;
|
.target_system = (uint8_t)status.system_id,
|
||||||
|
.target_component = (uint8_t)status.component_id
|
||||||
|
};
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
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);
|
||||||
@@ -551,20 +552,20 @@ int commander_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (!strcmp(argv[1], "transition")) {
|
if (!strcmp(argv[1], "transition")) {
|
||||||
|
|
||||||
vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.target_system = status.system_id;
|
.timestamp = 0,
|
||||||
cmd.target_component = status.component_id;
|
.param5 = NAN,
|
||||||
|
.param6 = NAN,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
|
/* transition to the other mode */
|
||||||
/* transition to the other mode */
|
.param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
|
||||||
cmd.param1 = (status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
.param2 = NAN,
|
||||||
/* param 2-3 unused */
|
.param3 = NAN,
|
||||||
cmd.param2 = NAN;
|
.param4 = NAN,
|
||||||
cmd.param3 = NAN;
|
.param7 = NAN,
|
||||||
cmd.param4 = NAN;
|
.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
|
||||||
cmd.param5 = NAN;
|
.target_system = (uint8_t)status.system_id,
|
||||||
cmd.param6 = NAN;
|
.target_component = (uint8_t)status.component_id
|
||||||
cmd.param7 = NAN;
|
};
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
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);
|
||||||
@@ -620,13 +621,20 @@ int commander_main(int argc, char *argv[])
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.target_system = status.system_id;
|
.timestamp = 0,
|
||||||
cmd.target_component = status.component_id;
|
.param5 = 0.0f,
|
||||||
|
.param6 = 0.0f,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
|
/* 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 */
|
.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f, /* lockdown */
|
||||||
cmd.param1 = strcmp(argv[2], "off") ? 2.0f : 0.0f; /* lockdown */
|
.param2 = 0.0f,
|
||||||
|
.param3 = 0.0f,
|
||||||
|
.param4 = 0.0f,
|
||||||
|
.param7 = 0.0f,
|
||||||
|
.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
|
||||||
|
.target_system = (uint8_t)status.system_id,
|
||||||
|
.target_component = (uint8_t)status.component_id
|
||||||
|
};
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
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);
|
||||||
@@ -1650,8 +1658,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
|
|
||||||
/* Subscribe to command topic */
|
/* Subscribe to command topic */
|
||||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||||
struct vehicle_command_s cmd;
|
|
||||||
memset(&cmd, 0, sizeof(cmd));
|
|
||||||
|
|
||||||
/* Subscribe to parameters changed topic */
|
/* Subscribe to parameters changed topic */
|
||||||
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||||
@@ -2986,6 +2992,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||||||
orb_check(cmd_sub, &updated);
|
orb_check(cmd_sub, &updated);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
|
struct vehicle_command_s cmd;
|
||||||
|
|
||||||
/* got command */
|
/* got command */
|
||||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||||
|
|
||||||
@@ -4125,9 +4133,11 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* publish ACK */
|
/* publish ACK */
|
||||||
vehicle_command_ack_s command_ack = {};
|
vehicle_command_ack_s command_ack = {
|
||||||
command_ack.command = cmd.command;
|
.timestamp = 0,
|
||||||
command_ack.result = result;
|
.command = cmd.command,
|
||||||
|
.result = (uint8_t)result
|
||||||
|
};
|
||||||
|
|
||||||
if (command_ack_pub != nullptr) {
|
if (command_ack_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
|
||||||
@@ -4144,8 +4154,6 @@ void *commander_low_prio_loop(void *arg)
|
|||||||
|
|
||||||
/* Subscribe to command topic */
|
/* Subscribe to command topic */
|
||||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||||
struct vehicle_command_s cmd;
|
|
||||||
memset(&cmd, 0, sizeof(cmd));
|
|
||||||
|
|
||||||
/* command ack */
|
/* command ack */
|
||||||
orb_advert_t command_ack_pub = nullptr;
|
orb_advert_t command_ack_pub = nullptr;
|
||||||
@@ -4166,6 +4174,7 @@ void *commander_low_prio_loop(void *arg)
|
|||||||
warn("commander: poll error %d, %d", pret, errno);
|
warn("commander: poll error %d, %d", pret, errno);
|
||||||
continue;
|
continue;
|
||||||
} else if (pret != 0) {
|
} else if (pret != 0) {
|
||||||
|
struct vehicle_command_s cmd;
|
||||||
|
|
||||||
/* if we reach here, we have a valid command */
|
/* if we reach here, we have a valid command */
|
||||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||||
|
|||||||
@@ -122,7 +122,6 @@ void SendEvent::cycle()
|
|||||||
|
|
||||||
void SendEvent::process_commands()
|
void SendEvent::process_commands()
|
||||||
{
|
{
|
||||||
struct vehicle_command_s cmd;
|
|
||||||
bool updated;
|
bool updated;
|
||||||
orb_check(_vehicle_command_sub, &updated);
|
orb_check(_vehicle_command_sub, &updated);
|
||||||
|
|
||||||
@@ -130,6 +129,8 @@ void SendEvent::process_commands()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
struct vehicle_command_s cmd;
|
||||||
|
|
||||||
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
|
||||||
|
|
||||||
bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
|
bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
|
||||||
@@ -167,12 +168,12 @@ void SendEvent::process_commands()
|
|||||||
|
|
||||||
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
|
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
|
||||||
{
|
{
|
||||||
struct vehicle_command_ack_s command_ack;
|
|
||||||
|
|
||||||
/* publish ACK */
|
/* publish ACK */
|
||||||
command_ack.command = cmd.command;
|
struct vehicle_command_ack_s command_ack = {
|
||||||
command_ack.result = result;
|
.timestamp = hrt_absolute_time(),
|
||||||
command_ack.timestamp = hrt_absolute_time();
|
.command = cmd.command,
|
||||||
|
.result = (uint8_t)result,
|
||||||
|
};
|
||||||
|
|
||||||
if (_command_ack_pub != nullptr) {
|
if (_command_ack_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
|
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
|
||||||
@@ -256,18 +257,17 @@ int SendEvent::custom_command(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.target_system = 0;
|
.timestamp = 0,
|
||||||
cmd.target_component = 0;
|
.param5 = (float)((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
|
||||||
|
.param6 = NAN,
|
||||||
cmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
|
.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
|
||||||
cmd.param1 = (gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN;
|
.param2 = NAN,
|
||||||
cmd.param2 = NAN;
|
.param3 = NAN,
|
||||||
cmd.param3 = NAN;
|
.param4 = NAN,
|
||||||
cmd.param4 = NAN;
|
.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
|
||||||
cmd.param5 = (accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN;
|
.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION
|
||||||
cmd.param6 = NAN;
|
};
|
||||||
cmd.param7 = (baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN;
|
|
||||||
|
|
||||||
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
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);
|
||||||
|
|||||||
@@ -2001,10 +2001,11 @@ int Logger::remove_directory(const char *dir)
|
|||||||
|
|
||||||
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result)
|
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result)
|
||||||
{
|
{
|
||||||
vehicle_command_ack_s vehicle_command_ack;
|
vehicle_command_ack_s vehicle_command_ack = {
|
||||||
vehicle_command_ack.timestamp = hrt_absolute_time();
|
.timestamp = hrt_absolute_time(),
|
||||||
vehicle_command_ack.command = command;
|
.command = command,
|
||||||
vehicle_command_ack.result = result;
|
.result = (uint8_t)result
|
||||||
|
};
|
||||||
|
|
||||||
if (vehicle_command_ack_pub == nullptr) {
|
if (vehicle_command_ack_pub == nullptr) {
|
||||||
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
||||||
|
|||||||
@@ -2200,7 +2200,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
|
|
||||||
/* send command ACK */
|
/* send command ACK */
|
||||||
uint16_t current_command_ack = 0;
|
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 (ack_sub->update(&ack_time, &command_ack)) {
|
||||||
if (!command_ack.from_external) {
|
if (!command_ack.from_external) {
|
||||||
|
|||||||
@@ -1410,19 +1410,19 @@ protected:
|
|||||||
|
|
||||||
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
|
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
|
||||||
|
|
||||||
vehicle_command_s cmd{};
|
struct vehicle_command_s cmd = {
|
||||||
|
.timestamp = 0,
|
||||||
cmd.target_system = mavlink_system.sysid;
|
.param5 = NAN,
|
||||||
cmd.target_component = MAV_COMP_ID_CAMERA;
|
.param6 = NAN,
|
||||||
cmd.command = MAV_CMD_IMAGE_START_CAPTURE;
|
.param1 = 0.0f, // all cameras
|
||||||
cmd.confirmation = 0;
|
.param2 = 0.0f, // duration 0 because only taking one picture
|
||||||
cmd.param1 = 0; // all cameras
|
.param3 = 1.0f, // only take one
|
||||||
cmd.param2 = 0; // duration 0 because only taking one picture
|
.param4 = NAN,
|
||||||
cmd.param3 = 1; // only take one
|
.param7 = NAN,
|
||||||
cmd.param4 = NAN;
|
.command = MAV_CMD_IMAGE_START_CAPTURE,
|
||||||
cmd.param5 = NAN;
|
.target_system = mavlink_system.sysid,
|
||||||
cmd.param6 = NAN;
|
.target_component = MAV_COMP_ID_CAMERA
|
||||||
cmd.param7 = NAN;
|
};
|
||||||
|
|
||||||
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
|
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
|
||||||
|
|
||||||
|
|||||||
@@ -439,41 +439,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|||||||
_mavlink->request_stop_ulog_streaming();
|
_mavlink->request_stop_ulog_streaming();
|
||||||
}
|
}
|
||||||
|
|
||||||
struct vehicle_command_s vcmd;
|
struct vehicle_command_s vcmd = {
|
||||||
|
.timestamp = hrt_absolute_time(),
|
||||||
memset(&vcmd, 0, sizeof(vcmd));
|
.param5 = cmd_mavlink.param5,
|
||||||
|
.param6 = cmd_mavlink.param6,
|
||||||
vcmd.timestamp = hrt_absolute_time();
|
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||||
|
.param1 = cmd_mavlink.param1,
|
||||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
.param2 = cmd_mavlink.param2,
|
||||||
vcmd.param1 = cmd_mavlink.param1;
|
.param3 = cmd_mavlink.param3,
|
||||||
|
.param4 = cmd_mavlink.param4,
|
||||||
vcmd.param2 = cmd_mavlink.param2;
|
.param7 = cmd_mavlink.param7,
|
||||||
|
// XXX do proper translation
|
||||||
vcmd.param3 = cmd_mavlink.param3;
|
.command = cmd_mavlink.command,
|
||||||
|
.target_system = cmd_mavlink.target_system,
|
||||||
vcmd.param4 = cmd_mavlink.param4;
|
.target_component = cmd_mavlink.target_component,
|
||||||
|
.source_system = msg->sysid,
|
||||||
vcmd.param5 = cmd_mavlink.param5;
|
.source_component = msg->compid,
|
||||||
|
.confirmation = cmd_mavlink.confirmation,
|
||||||
vcmd.param6 = cmd_mavlink.param6;
|
.from_external = 1
|
||||||
|
};
|
||||||
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;
|
|
||||||
|
|
||||||
if (_cmd_pub == nullptr) {
|
if (_cmd_pub == nullptr) {
|
||||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
_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:
|
out:
|
||||||
|
|
||||||
if (send_ack) {
|
if (send_ack) {
|
||||||
vehicle_command_ack_s command_ack;
|
vehicle_command_ack_s command_ack = {
|
||||||
command_ack.command = cmd_mavlink.command;
|
.timestamp = 0,
|
||||||
|
.command = cmd_mavlink.command,
|
||||||
if (ret == PX4_OK) {
|
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
|
||||||
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
};
|
||||||
|
|
||||||
} else {
|
|
||||||
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_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;
|
send_ack = false;
|
||||||
|
|
||||||
struct vehicle_command_s vcmd;
|
struct vehicle_command_s vcmd = {
|
||||||
|
.timestamp = hrt_absolute_time(),
|
||||||
memset(&vcmd, 0, sizeof(vcmd));
|
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
|
||||||
|
.param5 = ((double)cmd_mavlink.x) / 1e7,
|
||||||
vcmd.timestamp = hrt_absolute_time();
|
.param6 = ((double)cmd_mavlink.y) / 1e7,
|
||||||
|
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
||||||
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
|
.param1 = cmd_mavlink.param1,
|
||||||
vcmd.param1 = cmd_mavlink.param1;
|
.param2 = cmd_mavlink.param2,
|
||||||
|
.param3 = cmd_mavlink.param3,
|
||||||
vcmd.param2 = cmd_mavlink.param2;
|
.param4 = cmd_mavlink.param4,
|
||||||
|
.param7 = cmd_mavlink.z,
|
||||||
vcmd.param3 = cmd_mavlink.param3;
|
// XXX do proper translation
|
||||||
|
.command = cmd_mavlink.command,
|
||||||
vcmd.param4 = cmd_mavlink.param4;
|
.target_system = cmd_mavlink.target_system,
|
||||||
|
.target_component = cmd_mavlink.target_component,
|
||||||
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
|
.source_system = msg->sysid,
|
||||||
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
|
.source_component = msg->compid,
|
||||||
|
.confirmation = 0,
|
||||||
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
|
.from_external = 1
|
||||||
|
};
|
||||||
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;
|
|
||||||
|
|
||||||
if (_cmd_pub == nullptr) {
|
if (_cmd_pub == nullptr) {
|
||||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
_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:
|
out:
|
||||||
|
|
||||||
if (send_ack) {
|
if (send_ack) {
|
||||||
vehicle_command_ack_s command_ack;
|
vehicle_command_ack_s command_ack = {
|
||||||
command_ack.command = cmd_mavlink.command;
|
.timestamp = 0,
|
||||||
|
.command = cmd_mavlink.command,
|
||||||
if (ret == PX4_OK) {
|
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
|
||||||
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
};
|
||||||
|
|
||||||
} else {
|
|
||||||
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_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);
|
MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid);
|
||||||
|
|
||||||
vehicle_command_ack_s command_ack = {};
|
vehicle_command_ack_s command_ack = {
|
||||||
command_ack.command = ack.command;
|
.timestamp = hrt_absolute_time(),
|
||||||
command_ack.result = ack.result;
|
.command = ack.command,
|
||||||
command_ack.timestamp = hrt_absolute_time();
|
.result = ack.result,
|
||||||
command_ack.from_external = 1;
|
.from_external = 1
|
||||||
|
};
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
|
_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_set_mode_t new_mode;
|
||||||
mavlink_msg_set_mode_decode(msg, &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;
|
union px4_custom_mode custom_mode;
|
||||||
custom_mode.data = new_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;
|
struct vehicle_command_s vcmd = {
|
||||||
vcmd.param2 = custom_mode.main_mode;
|
.timestamp = hrt_absolute_time(),
|
||||||
vcmd.param3 = custom_mode.sub_mode;
|
.param5 = 0,
|
||||||
vcmd.param4 = 0;
|
.param6 = 0,
|
||||||
vcmd.param5 = 0;
|
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||||
vcmd.param6 = 0;
|
.param1 = (float)new_mode.base_mode,
|
||||||
vcmd.param7 = 0;
|
.param2 = (float)custom_mode.main_mode,
|
||||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
|
.param3 = (float)custom_mode.sub_mode,
|
||||||
vcmd.target_system = new_mode.target_system;
|
.param4 = 0,
|
||||||
vcmd.target_component = MAV_COMP_ID_ALL;
|
.param7 = 0,
|
||||||
vcmd.source_system = msg->sysid;
|
.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE,
|
||||||
vcmd.source_component = msg->compid;
|
.target_system = new_mode.target_system,
|
||||||
vcmd.confirmation = 1;
|
.target_component = MAV_COMP_ID_ALL,
|
||||||
vcmd.timestamp = hrt_absolute_time();
|
.source_system = msg->sysid,
|
||||||
vcmd.from_external = 1;
|
.source_component = msg->compid,
|
||||||
|
.confirmation = 1,
|
||||||
|
.from_external = 1
|
||||||
|
};
|
||||||
|
|
||||||
if (_cmd_pub == nullptr) {
|
if (_cmd_pub == nullptr) {
|
||||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||||
|
|||||||
@@ -476,11 +476,14 @@ MissionBlock::issue_command(const struct mission_item_s *item)
|
|||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
struct vehicle_command_s cmd = {};
|
|
||||||
mission_item_to_vehicle_command(item, &cmd);
|
|
||||||
const hrt_abstime now = hrt_absolute_time();
|
const hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
|
struct vehicle_command_s cmd = {
|
||||||
|
.timestamp = now
|
||||||
|
};
|
||||||
|
|
||||||
|
mission_item_to_vehicle_command(item, &cmd);
|
||||||
_action_start = now;
|
_action_start = now;
|
||||||
cmd.timestamp = now;
|
|
||||||
|
|
||||||
_navigator->publish_vehicle_cmd(cmd);
|
_navigator->publish_vehicle_cmd(cmd);
|
||||||
}
|
}
|
||||||
@@ -715,10 +718,17 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
|
|||||||
!_navigator->get_vstatus()->is_rotary_wing &&
|
!_navigator->get_vstatus()->is_rotary_wing &&
|
||||||
_param_force_vtol.get() == 1) {
|
_param_force_vtol.get() == 1) {
|
||||||
|
|
||||||
struct vehicle_command_s cmd = {};
|
struct vehicle_command_s cmd = {
|
||||||
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
|
.timestamp = hrt_absolute_time(),
|
||||||
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
.param5 = 0.0f,
|
||||||
cmd.timestamp = hrt_absolute_time();
|
.param6 = 0.0f,
|
||||||
|
.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC,
|
||||||
|
.param2 = 0.0f,
|
||||||
|
.param3 = 0.0f,
|
||||||
|
.param4 = 0.0f,
|
||||||
|
.param7 = 0.0f,
|
||||||
|
.command = NAV_CMD_DO_VTOL_TRANSITION
|
||||||
|
};
|
||||||
|
|
||||||
_navigator->publish_vehicle_cmd(cmd);
|
_navigator->publish_vehicle_cmd(cmd);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -375,7 +375,7 @@ Navigator::task_main()
|
|||||||
orb_check(_vehicle_command_sub, &updated);
|
orb_check(_vehicle_command_sub, &updated);
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
vehicle_command_s cmd = {};
|
vehicle_command_s cmd;
|
||||||
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
|
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
|
||||||
|
|
||||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
|
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
|
||||||
@@ -479,15 +479,24 @@ Navigator::task_main()
|
|||||||
int land_start = _mission.find_offboard_land_start();
|
int land_start = _mission.find_offboard_land_start();
|
||||||
|
|
||||||
if (land_start != -1) {
|
if (land_start != -1) {
|
||||||
vehicle_command_s cmd_mission_start = {};
|
struct vehicle_command_s vcmd = {};
|
||||||
cmd_mission_start.timestamp = hrt_absolute_time();
|
vcmd.timestamp = hrt_absolute_time(),
|
||||||
cmd_mission_start.target_system = get_vstatus()->system_id;
|
vcmd.param1 = (float)land_start,
|
||||||
cmd_mission_start.target_component = get_vstatus()->component_id;
|
vcmd.param2 = 0.0f,
|
||||||
cmd_mission_start.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
|
vcmd.param3 = 0.0f,
|
||||||
cmd_mission_start.param1 = land_start;
|
vcmd.param4 = 0.0f,
|
||||||
cmd_mission_start.param2 = 0;
|
vcmd.param5 = 0.0,
|
||||||
|
vcmd.param6 = 0.0,
|
||||||
|
vcmd.param7 = 0.0f,
|
||||||
|
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
|
||||||
|
vcmd.target_system = (uint8_t)get_vstatus()->system_id;
|
||||||
|
vcmd.target_component = (uint8_t)get_vstatus()->component_id;
|
||||||
|
vcmd.source_system = (uint8_t)get_vstatus()->system_id;
|
||||||
|
vcmd.source_component = (uint8_t)get_vstatus()->component_id;
|
||||||
|
vcmd.confirmation = false;
|
||||||
|
vcmd.from_external = false;
|
||||||
|
|
||||||
publish_vehicle_cmd(cmd_mission_start);
|
publish_vehicle_cmd(vcmd);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_WARN("planned landing not available");
|
PX4_WARN("planned landing not available");
|
||||||
|
|||||||
@@ -385,6 +385,8 @@ int sdlog2_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (!strncmp(argv[1], "on", 2)) {
|
if (!strncmp(argv[1], "on", 2)) {
|
||||||
struct vehicle_command_s cmd;
|
struct vehicle_command_s cmd;
|
||||||
|
|
||||||
|
memset(&cmd, 0, sizeof(cmd));
|
||||||
cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE;
|
cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE;
|
||||||
cmd.param1 = -1;
|
cmd.param1 = -1;
|
||||||
cmd.param2 = -1;
|
cmd.param2 = -1;
|
||||||
@@ -396,6 +398,8 @@ int sdlog2_main(int argc, char *argv[])
|
|||||||
|
|
||||||
if (!strcmp(argv[1], "off")) {
|
if (!strcmp(argv[1], "off")) {
|
||||||
struct vehicle_command_s cmd;
|
struct vehicle_command_s cmd;
|
||||||
|
|
||||||
|
memset(&cmd, 0, sizeof(cmd));
|
||||||
cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE;
|
cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE;
|
||||||
cmd.param1 = -1;
|
cmd.param1 = -1;
|
||||||
cmd.param2 = -1;
|
cmd.param2 = -1;
|
||||||
|
|||||||
@@ -534,9 +534,11 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Acknowledge the received command
|
// Acknowledge the received command
|
||||||
struct vehicle_command_ack_s ack = {};
|
struct vehicle_command_ack_s ack = {
|
||||||
ack.command = cmd.command;
|
.timestamp = 0,
|
||||||
ack.result = cmd_ack_result;
|
.command = cmd.command,
|
||||||
|
.result = cmd_ack_result
|
||||||
|
};
|
||||||
|
|
||||||
if (_command_ack_pub == nullptr) {
|
if (_command_ack_pub == nullptr) {
|
||||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||||
|
|||||||
@@ -57,6 +57,6 @@ private:
|
|||||||
int DefaultTest();
|
int DefaultTest();
|
||||||
int PingPongTest();
|
int PingPongTest();
|
||||||
struct esc_status_s m_esc_status;
|
struct esc_status_s m_esc_status;
|
||||||
struct vehicle_command_s m_vc;
|
struct vehicle_command_s m_vc = {};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -54,6 +54,6 @@ private:
|
|||||||
int uSleepTest();
|
int uSleepTest();
|
||||||
|
|
||||||
struct esc_status_s m_esc_status;
|
struct esc_status_s m_esc_status;
|
||||||
struct vehicle_command_s m_vc;
|
struct vehicle_command_s m_vc = {};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user