dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add support to new fields in command_ack

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/vehicle_command_ack.msg
View File

@ -10,3 +10,7 @@ uint32 ORB_QUEUE_LENGTH = 3
uint16 command
uint8 result
uint8 from_external
uint8 result_param1
int32 result_param2
uint8 target_system
uint8 target_component

7
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -724,8 +724,13 @@ CameraTrigger::cycle_trampoline(void *arg)
if (updated && need_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)cmd_result
.result = (uint8_t)cmd_result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (trig->_cmd_ack_pub == nullptr) {

16
src/drivers/vmount/input_mavlink.cpp
View File

@ -41,7 +41,6 @@
#include "input_mavlink.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <drivers/drv_hrt.h>
@ -292,7 +291,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
break;
}
_ack_vehicle_command(vehicle_command.command);
_ack_vehicle_command(&vehicle_command);
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
@ -301,7 +300,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
*control_data = &_control_data;
_ack_vehicle_command(vehicle_command.command);
_ack_vehicle_command(&vehicle_command);
}
}
@ -310,12 +309,17 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
return 0;
}
void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
{
vehicle_command_ack_s vehicle_command_ack = {
.timestamp = hrt_absolute_time(),
.command = command,
.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED
.result_param2 = 0,
.command = cmd->command,
.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd->source_system,
.target_component = cmd->source_component
};
if (_vehicle_command_ack_pub == nullptr) {

6
src/drivers/vmount/input_mavlink.h
View File

@ -41,9 +41,11 @@
#include "input.h"
#include "input_rc.h"
#include <uORB/topics/vehicle_roi.h>
#include <cstdint>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command.h>
namespace vmount
{
@ -90,7 +92,7 @@ protected:
virtual int initialize();
private:
void _ack_vehicle_command(uint16_t command);
void _ack_vehicle_command(vehicle_command_s *cmd);
int _vehicle_command_sub = -1;
orb_advert_t _vehicle_command_ack_pub = nullptr;

7
src/modules/commander/commander.cpp
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) {

5
src/modules/events/send_event.cpp
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) {

20
src/modules/logger/logger.cpp
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) {

4
src/modules/logger/logger.h
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

4
src/modules/mavlink/mavlink_main.cpp
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);

20
src/modules/mavlink/mavlink_receiver.cpp
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) {

7
src/modules/uavcan/uavcan_servers.cpp
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) {