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

Add support to new fields in command_ack

Browse Source
This commit is contained in:
José Roberto de Souza
2017-08-18 16:52:46 -07:00
committed by Lorenz Meier
parent d640d1aaf1
commit 4462869432
11 changed files with 77 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+6 -1
View File
@@ -4135,8 +4135,13 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
/* publish ACK */
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)result
.result = (uint8_t)result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (command_ack_pub != nullptr) {
src/modules/events/send_event.cpp
+5
View File
@@ -171,8 +171,13 @@ 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 = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (_command_ack_pub != nullptr) {
src/modules/logger/logger.cpp
+12 -8
View File
@@ -49,7 +49,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <drivers/drv_hrt.h>
@@ -867,22 +866,22 @@ void Logger::run()
if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
if ((int)(command.param1 + 0.5f) != 0) {
ack_vehicle_command(vehicle_command_ack_pub, command.command,
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
} else if (can_start_mavlink_log()) {
ack_vehicle_command(vehicle_command_ack_pub, command.command,
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
start_log_mavlink();
} else {
ack_vehicle_command(vehicle_command_ack_pub, command.command,
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
}
} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
stop_log_mavlink();
ack_vehicle_command(vehicle_command_ack_pub, command.command,
ack_vehicle_command(vehicle_command_ack_pub, &command,
vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
}
@@ -1999,12 +1998,17 @@ int Logger::remove_directory(const char *dir)
return ret;
}
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, vehicle_command_s *cmd, uint32_t result)
{
vehicle_command_ack_s vehicle_command_ack = {
.timestamp = hrt_absolute_time(),
.command = command,
.result = (uint8_t)result
.result_param2 = 0,
.command = cmd->command,
.result = (uint8_t)result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd->source_system,
.target_component = cmd->source_component
};
if (vehicle_command_ack_pub == nullptr) {
src/modules/logger/logger.h
+3 -1
View File
@@ -43,6 +43,8 @@
#include <systemlib/printload.h>
#include <px4_module.h>
#include <uORB/topics/vehicle_command.h>
extern "C" __EXPORT int logger_main(int argc, char *argv[]);
#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic
@@ -269,7 +271,7 @@ private:
void add_thermal_calibration_topics();
void add_system_identification_topics();
void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result);
void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result);
/**
* initialize the output for the process load, so that ~1 second later it will be written to the log
src/modules/mavlink/mavlink_main.cpp
+4
View File
@@ -2207,6 +2207,10 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_command_ack_t msg;
msg.result = command_ack.result;
msg.command = command_ack.command;
msg.progress = command_ack.result_param1;
msg.result_param2 = command_ack.result_param2;
msg.target_system = command_ack.target_system;
msg.target_component = command_ack.target_component;
current_command_ack = command_ack.command;
mavlink_msg_command_ack_send_struct(get_channel(), &msg);
src/modules/mavlink/mavlink_receiver.cpp
+17 -3
View File
@@ -472,8 +472,13 @@ out:
if (send_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.from_external = 0,
.result_param1 = 0,
.target_system = msg->sysid,
.target_component = msg->compid
};
if (_command_ack_pub == nullptr) {
@@ -564,8 +569,13 @@ out:
if (send_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd_mavlink.command,
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED)
.result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED),
.from_external = 0,
.result_param1 = 0,
.target_system = msg->sysid,
.target_component = msg->compid
};
if (_command_ack_pub == nullptr) {
@@ -588,9 +598,13 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
vehicle_command_ack_s command_ack = {
.timestamp = hrt_absolute_time(),
.result_param2 = ack.result_param2,
.command = ack.command,
.result = ack.result,
.from_external = 1
.from_external = 1,
.result_param1 = ack.progress,
.target_system = ack.target_system,
.target_component = ack.target_component
};
if (_command_ack_pub == nullptr) {
src/modules/uavcan/uavcan_servers.cpp
+6 -1
View File
@@ -536,8 +536,13 @@ 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
.result = cmd_ack_result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (_command_ack_pub == nullptr) {