diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 276b3d2564..27a43a293e 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -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, diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5045085b8a..5d7a8c5d37 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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); diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index cbfcc59c64..94333f443e 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -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, diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp index 3baa6dc110..3c30bc3895 100644 --- a/src/drivers/vmount/output_mavlink.cpp +++ b/src/drivers/vmount/output_mavlink.cpp @@ -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]; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 1b7795d21e..abf9fde727 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -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) { struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 88cd7d8a4f..0b24522006 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -498,21 +498,20 @@ int commander_main(int argc, char *argv[]) if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { - vehicle_command_s cmd = {}; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - - cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - cmd.param1 = NAN; /* minimum pitch */ - /* param 2-3 unused */ - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; - - cmd.timestamp = hrt_absolute_time(); + struct vehicle_command_s cmd = { + .timestamp = hrt_absolute_time(), + .param5 = NAN, + .param6 = NAN, + /* minimum pitch */ + .param1 = NAN, + .param2 = NAN, + .param3 = NAN, + .param4 = NAN, + .param7 = NAN, + .command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF, + .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); (void)orb_unadvertise(h); @@ -530,18 +529,20 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "land")) { - vehicle_command_s cmd = {}; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - - cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - /* param 2-3 unused */ - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; + struct vehicle_command_s cmd = { + .timestamp = 0, + .param5 = NAN, + .param6 = NAN, + /* minimum pitch */ + .param1 = NAN, + .param2 = NAN, + .param3 = NAN, + .param4 = NAN, + .param7 = NAN, + .command = vehicle_command_s::VEHICLE_CMD_NAV_LAND, + .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); (void)orb_unadvertise(h); @@ -551,20 +552,20 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "transition")) { - vehicle_command_s cmd = {}; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION; - /* transition to the other mode */ - cmd.param1 = (status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - /* param 2-3 unused */ - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; + struct vehicle_command_s cmd = { + .timestamp = 0, + .param5 = NAN, + .param6 = 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 = (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); (void)orb_unadvertise(h); @@ -620,13 +621,20 @@ int commander_main(int argc, char *argv[]) return 1; } - vehicle_command_s cmd = {}; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; - /* 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 */ + struct vehicle_command_s cmd = { + .timestamp = 0, + .param5 = 0.0f, + .param6 = 0.0f, + /* 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 = (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); (void)orb_unadvertise(h); @@ -1650,8 +1658,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); /* Subscribe to parameters changed topic */ 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); if (updated) { + struct vehicle_command_s cmd; + /* got command */ 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 */ - vehicle_command_ack_s command_ack = {}; - command_ack.command = cmd.command; - command_ack.result = result; + vehicle_command_ack_s command_ack = { + .timestamp = 0, + .command = cmd.command, + .result = (uint8_t)result + }; if (command_ack_pub != nullptr) { 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 */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); /* command ack */ 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); continue; } else if (pret != 0) { + struct vehicle_command_s cmd; /* if we reach here, we have a valid command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index d7908131f2..5c3c2b49fb 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -122,7 +122,6 @@ void SendEvent::cycle() void SendEvent::process_commands() { - struct vehicle_command_s cmd; bool updated; orb_check(_vehicle_command_sub, &updated); @@ -130,6 +129,8 @@ void SendEvent::process_commands() return; } + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); 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) { - struct vehicle_command_ack_s command_ack; - /* publish ACK */ - command_ack.command = cmd.command; - command_ack.result = result; - command_ack.timestamp = hrt_absolute_time(); + struct vehicle_command_ack_s command_ack = { + .timestamp = hrt_absolute_time(), + .command = cmd.command, + .result = (uint8_t)result, + }; if (_command_ack_pub != nullptr) { 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 = {}; - cmd.target_system = 0; - cmd.target_component = 0; - - cmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - cmd.param1 = (gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN; - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = (accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN; - cmd.param6 = NAN; - cmd.param7 = (baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN; + struct vehicle_command_s cmd = { + .timestamp = 0, + .param5 = (float)((accel_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN), + .param6 = 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 + }; orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(h); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 839d0d1e8b..4604e373c1 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -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) { - vehicle_command_ack_s vehicle_command_ack; - vehicle_command_ack.timestamp = hrt_absolute_time(); - vehicle_command_ack.command = command; - vehicle_command_ack.result = result; + vehicle_command_ack_s vehicle_command_ack = { + .timestamp = hrt_absolute_time(), + .command = command, + .result = (uint8_t)result + }; if (vehicle_command_ack_pub == nullptr) { vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2eba718e93..472f8952ee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d382803c2e..b7d5772a30 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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()); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fff5d55d7b..4ec488feb5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b60dcf3550..35a7e896c9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -476,11 +476,14 @@ MissionBlock::issue_command(const struct mission_item_s *item) } } else { - struct vehicle_command_s cmd = {}; - mission_item_to_vehicle_command(item, &cmd); const hrt_abstime now = hrt_absolute_time(); + + struct vehicle_command_s cmd = { + .timestamp = now + }; + + mission_item_to_vehicle_command(item, &cmd); _action_start = now; - cmd.timestamp = now; _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 && _param_force_vtol.get() == 1) { - struct vehicle_command_s cmd = {}; - cmd.command = NAV_CMD_DO_VTOL_TRANSITION; - cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - cmd.timestamp = hrt_absolute_time(); + struct vehicle_command_s cmd = { + .timestamp = hrt_absolute_time(), + .param5 = 0.0f, + .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); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfebbd1c82..cbc27c67ab 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -375,7 +375,7 @@ Navigator::task_main() orb_check(_vehicle_command_sub, &updated); if (updated) { - vehicle_command_s cmd = {}; + vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); 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(); if (land_start != -1) { - vehicle_command_s cmd_mission_start = {}; - cmd_mission_start.timestamp = hrt_absolute_time(); - cmd_mission_start.target_system = get_vstatus()->system_id; - cmd_mission_start.target_component = get_vstatus()->component_id; - cmd_mission_start.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; - cmd_mission_start.param1 = land_start; - cmd_mission_start.param2 = 0; + struct vehicle_command_s vcmd = {}; + vcmd.timestamp = hrt_absolute_time(), + vcmd.param1 = (float)land_start, + vcmd.param2 = 0.0f, + vcmd.param3 = 0.0f, + vcmd.param4 = 0.0f, + 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 { PX4_WARN("planned landing not available"); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 24d55c1b11..de54e22f7f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -385,6 +385,8 @@ int sdlog2_main(int argc, char *argv[]) if (!strncmp(argv[1], "on", 2)) { struct vehicle_command_s cmd; + + memset(&cmd, 0, sizeof(cmd)); cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; cmd.param1 = -1; cmd.param2 = -1; @@ -396,6 +398,8 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "off")) { struct vehicle_command_s cmd; + + memset(&cmd, 0, sizeof(cmd)); cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; cmd.param1 = -1; cmd.param2 = -1; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ef7439f70d..e8f6742861 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -534,9 +534,11 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } // Acknowledge the received command - struct vehicle_command_ack_s ack = {}; - ack.command = cmd.command; - ack.result = cmd_ack_result; + struct vehicle_command_ack_s ack = { + .timestamp = 0, + .command = cmd.command, + .result = cmd_ack_result + }; if (_command_ack_pub == nullptr) { _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h index 4f4dcf02d6..f706cbaf89 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.h +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -57,6 +57,6 @@ private: int DefaultTest(); int PingPongTest(); struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc; + struct vehicle_command_s m_vc = {}; }; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 68236eb854..aea7646bc9 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -54,6 +54,6 @@ private: int uSleepTest(); struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc; + struct vehicle_command_s m_vc = {}; };