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

stop ignoring Wmissing-field-initializers and fix (#9912)

Browse Source
This commit is contained in:
Daniel Agar
2018-07-22 12:45:52 -04:00
committed by GitHub
parent fc65939f0e
commit b1d3bb0403
21 changed files with 271 additions and 423 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+6 -10
View File
@@ -429,16 +429,12 @@ void AttitudeEstimatorQ::task_main()
last_time = now;
if (update(dt)) {
vehicle_attitude_s att = {
.timestamp = sensors.timestamp,
.rollspeed = _rates(0),
.pitchspeed = _rates(1),
.yawspeed = _rates(2),
.q = {_q(0), _q(1), _q(2), _q(3)},
.delta_q_reset = {},
.quat_reset_counter = 0,
};
vehicle_attitude_s att = {};
att.timestamp = sensors.timestamp;
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
_q.copyTo(att.q);
/* the instance count is not used here */
int att_inst;
src/modules/commander/arm_auth.cpp
+6 -14
View File
@@ -78,23 +78,15 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
static void arm_auth_request_msg_send()
{
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = 0,
.param6 = 0,
.param1 = 0,
.param2 = 0,
.param3 = 0,
.param4 = 0,
.param7 = 0,
.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST,
.target_system = arm_parameters.authorizer_system_id
};
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), &cmd);
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &cmd);
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
}
}
src/modules/commander/commander.cpp
+53 -100
View File
@@ -263,6 +263,26 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
return ret;
}
static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
{
vehicle_command_s vcmd = {};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = param1;
vcmd.param2 = param2;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param5 = (double)NAN;
vcmd.param6 = (double)NAN;
vcmd.param7 = NAN;
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
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);
return (h != nullptr);
}
int commander_main(int argc, char *argv[])
{
if (argc < 2) {
@@ -273,7 +293,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
PX4_INFO("already running");
/* this is not an error */
return 0;
}
@@ -299,7 +319,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running) {
warnx("commander already stopped");
PX4_WARN("already stopped");
return 0;
}
@@ -307,14 +327,14 @@ int commander_main(int argc, char *argv[])
Commander::main(argc, argv);
warnx("terminated.");
PX4_INFO("terminated.");
return 0;
}
/* commands needing the app to run below */
if (!thread_running) {
warnx("\tcommander not started");
PX4_ERR("not started");
return 1;
}
@@ -339,17 +359,17 @@ int commander_main(int argc, char *argv[])
} else if (!strcmp(argv[2], "airspeed")) {
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else {
warnx("argument %s unsupported.", argv[2]);
PX4_ERR("argument %s unsupported.", argv[2]);
}
if (calib_ret) {
warnx("calibration failed, exiting.");
PX4_ERR("calibration failed, exiting.");
return 1;
} else {
return 0;
}
} else {
warnx("missing argument");
PX4_ERR("missing argument");
}
}
@@ -365,98 +385,51 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "arm")) {
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
warnx("arming failed");
PX4_ERR("arming failed");
}
return 0;
}
if (!strcmp(argv[1], "disarm")) {
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
warnx("rejected disarm");
PX4_ERR("rejected disarm");
}
return 0;
}
if (!strcmp(argv[1], "takeoff")) {
bool ret = false;
/* see if we got a home position */
if (status_flags.condition_local_position_valid) {
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
struct vehicle_command_s cmd = {
.timestamp = hrt_absolute_time(),
.param5 = (double)NAN,
.param6 = (double)NAN,
/* minimum pitch */
.param1 = NAN,
.param2 = NAN,
.param3 = NAN,
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
} else {
warnx("arming failed");
PX4_ERR("arming failed");
}
} else {
warnx("rejecting takeoff, no position lock yet. Please retry..");
PX4_ERR("rejecting takeoff, no position lock yet. Please retry..");
}
return 0;
return (ret ? 0 : 1);
}
if (!strcmp(argv[1], "land")) {
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND);
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = (double)NAN,
.param6 = (double)NAN,
/* minimum pitch */
.param1 = NAN,
.param2 = NAN,
.param3 = NAN,
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND,
.target_system = status.system_id,
.target_component = status.component_id
};
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
return 0;
return (ret ? 0 : 1);
}
if (!strcmp(argv[1], "transition")) {
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = (double)NAN,
.param6 = (double)NAN,
/* 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),
.param2 = NAN,
.param3 = NAN,
.param4 = NAN,
.param7 = NAN,
.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
.target_system = status.system_id,
.target_component = status.component_id
};
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
(float)(status.is_rotary_wing ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC));
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
return 0;
return (ret ? 0 : 1);
}
if (!strcmp(argv[1], "mode")) {
@@ -489,16 +462,16 @@ int commander_main(int argc, char *argv[])
} else if (!strcmp(argv[2], "auto:precland")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
} else {
warnx("argument %s unsupported.", argv[2]);
PX4_ERR("argument %s unsupported.", argv[2]);
}
if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) {
warnx("mode change failed");
PX4_ERR("mode change failed");
}
return 0;
} else {
warnx("missing argument");
PX4_ERR("missing argument");
}
}
@@ -509,25 +482,10 @@ int commander_main(int argc, char *argv[])
return 1;
}
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = 0.0,
.param6 = 0.0,
/* if the comparison matches for off (== 0) set 0.0f, 2.0f (on) else */
.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 = status.system_id,
.target_component = status.component_id
};
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
strcmp(argv[2], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f);
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
return 0;
return (ret ? 0 : 1);
}
usage("unrecognized command");
@@ -3700,23 +3658,18 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
}
/* publish ACK */
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)result,
.from_external = false,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
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 = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
}
src/modules/events/send_event.cpp
+17 -25
View File
@@ -194,16 +194,12 @@ void SendEvent::process_commands()
void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
{
/* publish ACK */
struct vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)result,
.from_external = false,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
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);
@@ -214,8 +210,6 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
}
}
int SendEvent::print_usage(const char *reason)
{
if (reason) {
@@ -287,20 +281,18 @@ int SendEvent::custom_command(int argc, char *argv[])
}
}
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = ((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN),
.param6 = (double)NAN,
.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
.param2 = NAN,
.param3 = NAN,
.param4 = NAN,
.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN),
.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION
};
vehicle_command_s vcmd = {};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.param2 = NAN;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param5 = ((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
vcmd.param6 = (double)NAN;
vcmd.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
print_usage("unrecognized command");
src/modules/logger/logger.cpp
+7 -14
View File
@@ -2228,25 +2228,18 @@ int Logger::remove_directory(const char *dir)
void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result)
{
vehicle_command_ack_s vehicle_command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = cmd->command,
.result = (uint8_t)result,
.from_external = false,
.result_param1 = 0,
.target_system = cmd->source_system,
.target_component = cmd->source_component
};
vehicle_command_ack_s vehicle_command_ack = {};
vehicle_command_ack.timestamp = hrt_absolute_time();
vehicle_command_ack.command = cmd->command;
vehicle_command_ack.result = (uint8_t)result;
vehicle_command_ack.target_system = cmd->source_system;
vehicle_command_ack.target_component = cmd->source_component;
if (vehicle_command_ack_pub == nullptr) {
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
}
}
} // namespace logger
src/modules/mavlink/mavlink_main.cpp
+7 -10
View File
@@ -2288,16 +2288,13 @@ Mavlink::task_main(int argc, char *argv[])
}
// send positive command ack
struct vehicle_command_ack_s command_ack = {
.timestamp = vehicle_cmd.timestamp,
.result_param2 = 0,
.command = vehicle_cmd.command,
.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
.from_external = !vehicle_cmd.from_external,
.result_param1 = 0,
.target_system = vehicle_cmd.source_system,
.target_component = vehicle_cmd.source_component
};
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = vehicle_cmd.timestamp;
command_ack.command = vehicle_cmd.command;
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
command_ack.from_external = !vehicle_cmd.from_external;
command_ack.target_system = vehicle_cmd.source_system;
command_ack.target_component = vehicle_cmd.source_component;
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
src/modules/mavlink/mavlink_messages.cpp
+14 -15
View File
@@ -1551,25 +1551,24 @@ protected:
mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
struct vehicle_command_s cmd = {
.timestamp = 0,
.param5 = (double)NAN,
.param6 = (double)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
};
vehicle_command_s vcmd = {};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = 0.0f; // all cameras
vcmd.param2 = 0.0f; // duration 0 because only taking one picture
vcmd.param3 = 1.0f; // only take one
vcmd.param4 = NAN;
vcmd.param5 = (double)NAN;
vcmd.param6 = (double)NAN;
vcmd.param7 = NAN;
vcmd.command = MAV_CMD_IMAGE_START_CAPTURE;
vcmd.target_system = mavlink_system.sysid;
vcmd.target_component = MAV_COMP_ID_CAMERA;
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel());
// TODO: move this camera_trigger and publish as a vehicle_command
/* send MAV_CMD_DO_DIGICAM_CONTROL*/
mavlink_command_long_t digicam_ctrl_cmd;
mavlink_command_long_t digicam_ctrl_cmd = {};
digicam_ctrl_cmd.target_system = 0; // 0 for broadcast
digicam_ctrl_cmd.target_component = MAV_COMP_ID_CAMERA;
src/modules/mavlink/mavlink_receiver.cpp
+67 -76
View File
@@ -174,16 +174,12 @@ MavlinkReceiver::~MavlinkReceiver()
void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
{
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = command,
.result = result,
.from_external = false,
.result_param1 = 0,
.target_system = sysid,
.target_component = compid
};
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command;
command_ack.result = result;
command_ack.target_system = sysid;
command_ack.target_component = compid;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -490,24 +486,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = (double)cmd_mavlink.param5,
.param6 = (double)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,
.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 = true
};
vehicle_command_s 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 = (double)cmd_mavlink.param5;
vcmd.param6 = (double)cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
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 = true;
handle_message_command_both(msg, cmd_mavlink, vcmd);
}
@@ -518,30 +515,28 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink);
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,
.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 = false,
.from_external = true
};
vehicle_command_s 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;
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z;
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 = false;
vcmd.from_external = true;
handle_message_command_both(msg, cmd_mavlink, vcmd);
}
template <class T>
void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command)
@@ -636,16 +631,15 @@ 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 = {
.timestamp = hrt_absolute_time(),
.result_param2 = ack.result_param2,
.command = ack.command,
.result = ack.result,
.from_external = true,
.result_param1 = ack.progress,
.target_system = ack.target_system,
.target_component = ack.target_component
};
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.result_param2 = ack.result_param2;
command_ack.command = ack.command;
command_ack.result = ack.result;
command_ack.from_external = true;
command_ack.result_param1 = ack.progress;
command_ack.target_system = ack.target_system;
command_ack.target_component = ack.target_component;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -785,24 +779,21 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
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 = true
};
vehicle_command_s vcmd = {};
vcmd.timestamp = hrt_absolute_time();
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = (float)new_mode.base_mode;
vcmd.param2 = (float)custom_mode.main_mode;
vcmd.param3 = (float)custom_mode.sub_mode;
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 = true;
vcmd.from_external = true;
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
src/modules/uavcan/uavcan_servers.cpp
+7 -10
View File
@@ -32,6 +32,7 @@
****************************************************************************/
#include <px4_tasks.h>
#include <drivers/drv_hrt.h>
#include <nuttx/config.h>
@@ -563,16 +564,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
}
// Acknowledge the received command
struct vehicle_command_ack_s ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = cmd_ack_result,
.from_external = false,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
vehicle_command_ack_s ack = {};
ack.timestamp = hrt_absolute_time();
ack.command = cmd.command;
ack.result = cmd_ack_result;
ack.target_system = cmd.source_system;
ack.target_component = cmd.source_component;
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
src/modules/vtol_att_control/vtol_att_control_main.cpp
+6 -11
View File
@@ -363,16 +363,12 @@ VtolAttitudeControl::handle_command()
// This might not be optimal but is better than no response at all.
if (_vehicle_cmd.from_external) {
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = 0,
.command = _vehicle_cmd.command,
.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
.from_external = false,
.result_param1 = 0,
.target_system = _vehicle_cmd.source_system,
.target_component = _vehicle_cmd.source_component
};
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = _vehicle_cmd.command;
command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
command_ack.target_system = _vehicle_cmd.source_system;
command_ack.target_component = _vehicle_cmd.source_component;
if (_v_cmd_ack_pub == nullptr) {
_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
@@ -380,7 +376,6 @@ VtolAttitudeControl::handle_command()
} else {
orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
}
}
}