fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-14 23:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: receiver handle_message_command_both() use case statement

Browse Source
This commit is contained in:
Daniel Agar
2021-12-05 18:12:04 -05:00
parent 10f2564ae8
commit f56640f072
2 changed files with 264 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+260 -224
View File
@@ -383,6 +383,17 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink; mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink); mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (!evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component)) {
// Reject alien commands only if there is no forwarding or we've never seen target component before
if (!_mavlink->get_forwarding_on()
|| !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
}
return;
}
vehicle_command_s vcmd{}; vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time(); vcmd.timestamp = hrt_absolute_time();
@@ -415,7 +426,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.confirmation = cmd_mavlink.confirmation; vcmd.confirmation = cmd_mavlink.confirmation;
vcmd.from_external = true; vcmd.from_external = true;
handle_message_command_both(msg, cmd_mavlink, vcmd); handle_message_command_both(msg, cmd_mavlink.command, vcmd);
} }
void void
@@ -425,6 +436,17 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
mavlink_command_int_t cmd_mavlink; mavlink_command_int_t cmd_mavlink;
mavlink_msg_command_int_decode(msg, &cmd_mavlink); mavlink_msg_command_int_decode(msg, &cmd_mavlink);
if (!evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component)) {
// Reject alien commands only if there is no forwarding or we've never seen target component before
if (!_mavlink->get_forwarding_on()
|| !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
}
return;
}
vehicle_command_s vcmd{}; vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time(); vcmd.timestamp = hrt_absolute_time();
@@ -460,266 +482,280 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
vcmd.confirmation = false; vcmd.confirmation = false;
vcmd.from_external = true; vcmd.from_external = true;
handle_message_command_both(msg, cmd_mavlink, vcmd); handle_message_command_both(msg, cmd_mavlink.command, vcmd);
} }
template <class T> void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, uint16_t command,
void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command) const vehicle_command_s &vehicle_command)
{ {
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
bool send_ack = true; PX4_WARN("ignoring CMD with same SYS/COMP (%" PRIu8 "/%" PRIu8 ") ID", mavlink_system.sysid, mavlink_system.compid);
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
if (!target_ok) {
// Reject alien commands only if there is no forwarding or we've never seen target component before
if (!_mavlink->get_forwarding_on()
|| !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
}
return; return;
} }
// First we handle legacy support requests which were used before we had switch (command) {
// the generic MAV_CMD_REQUEST_MESSAGE. case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { acknowledge(msg->sysid, msg->compid, command,
result = handle_request_message_command(MAVLINK_MSG_ID_AUTOPILOT_VERSION); handle_request_message_command(MAVLINK_MSG_ID_AUTOPILOT_VERSION));
return;
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { case MAV_CMD_REQUEST_PROTOCOL_VERSION:
result = handle_request_message_command(MAVLINK_MSG_ID_PROTOCOL_VERSION); acknowledge(msg->sysid, msg->compid, command,
handle_request_message_command(MAVLINK_MSG_ID_PROTOCOL_VERSION));
return;
} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { case MAV_CMD_GET_HOME_POSITION:
result = handle_request_message_command(MAVLINK_MSG_ID_HOME_POSITION); acknowledge(msg->sysid, msg->compid, command,
handle_request_message_command(MAVLINK_MSG_ID_HOME_POSITION));
return;
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) { case MAV_CMD_REQUEST_FLIGHT_INFORMATION:
result = handle_request_message_command(MAVLINK_MSG_ID_FLIGHT_INFORMATION); acknowledge(msg->sysid, msg->compid, command,
handle_request_message_command(MAVLINK_MSG_ID_FLIGHT_INFORMATION));
return;
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) { case MAV_CMD_REQUEST_STORAGE_INFORMATION:
result = handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION); acknowledge(msg->sysid, msg->compid, command,
handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION));
return;
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { case MAV_CMD_SET_MESSAGE_INTERVAL:
if (set_message_interval((int)roundf(cmd_mavlink.param1), cmd_mavlink.param2, cmd_mavlink.param3)) { if (set_message_interval((int)roundf(vehicle_command.param1), vehicle_command.param2, vehicle_command.param3)) {
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED);
}
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
get_message_interval((int)roundf(cmd_mavlink.param1));
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
result = handle_request_message_command(message_id,
vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
} else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) {
struct actuator_controls_s actuator_controls = {};
actuator_controls.timestamp = hrt_absolute_time();
for (size_t i = 0; i < 8; i++) {
actuator_controls.control[i] = NAN;
}
switch ((int)(cmd_mavlink.param1 + 0.5f)) {
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE:
actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = cmd_mavlink.param2 / 50.0f - 1.0f;
break;
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_STEP:
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS:
case vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH:
default:
send_ack = false;
}
_actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls);
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
if (_mavlink->failure_injection_enabled()) {
_cmd_pub.publish(vehicle_command);
send_ack = false;
} else { } else {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
send_ack = true;
} }
} else if (cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) { return;
// since we're only paying attention to 3 AUX outputs, the
// index should be 0, otherwise ignore the message case MAV_CMD_GET_MESSAGE_INTERVAL:
if (((int) vehicle_command.param7) == 0) { get_message_interval((int)roundf(vehicle_command.param1));
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED);
return;
case MAV_CMD_REQUEST_MESSAGE:
if (vehicle_command.param1 >= 0 && vehicle_command.param1 <= UINT16_MAX) {
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
uint8_t result = handle_request_message_command(message_id,
vehicle_command.param2, vehicle_command.param3, vehicle_command.param4,
vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
acknowledge(msg->sysid, msg->compid, command, result);
} else {
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
}
return;
case MAV_CMD_SET_CAMERA_ZOOM:
// param1: camera_zoom_type
if ((int)roundf(vehicle_command.param1) == vehicle_command_s::VEHICLE_CAMERA_ZOOM_TYPE_RANGE) {
actuator_controls_s actuator_controls{}; actuator_controls_s actuator_controls{};
// update with existing values to avoid changing unspecified controls
_actuator_controls_3_sub.update(&actuator_controls); for (size_t i = 0; i < (sizeof(actuator_controls.control) / sizeof(actuator_controls.control[0])); i++) {
actuator_controls.control[i] = NAN;
}
actuator_controls.control[actuator_controls_s::INDEX_CAMERA_ZOOM] = vehicle_command.param2 / 50.0f - 1.0f;
actuator_controls.timestamp = hrt_absolute_time(); actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls);
bool updated = false; acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED);
if (PX4_ISFINITE(vehicle_command.param1)) { } else {
actuator_controls.control[5] = vehicle_command.param1; // no acknowledge, but republish
updated = true;
}
if (PX4_ISFINITE(vehicle_command.param2)) {
actuator_controls.control[6] = vehicle_command.param2;
updated = true;
}
if (PX4_ISFINITE(vehicle_command.param3)) {
actuator_controls.control[7] = vehicle_command.param3;
updated = true;
}
if (updated) {
_actuator_controls_pubs[3].publish(actuator_controls);
}
} }
_cmd_pub.publish(vehicle_command); _cmd_pub.publish(vehicle_command);
return;
} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) { case MAV_CMD_INJECT_FAILURE:
if (_mavlink->failure_injection_enabled()) {
bool has_module = true; _cmd_pub.publish(vehicle_command);
autotune_attitude_control_status_s status{};
_autotune_attitude_control_status_sub.copy(&status);
// if not busy enable via the parameter
// do not check the return value of the uORB copy above because the module
// starts publishing only when MC_AT_START is set
if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (!vehicle_status.in_transition_mode) {
param_t atune_start;
switch (vehicle_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
atune_start = param_find("FW_AT_START");
break;
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
atune_start = param_find("MC_AT_START");
break;
default:
atune_start = PARAM_INVALID;
break;
}
if (atune_start == PARAM_INVALID) {
has_module = false;
} else {
int32_t start = 1;
param_set(atune_start, &start);
}
} else {
has_module = false;
}
}
if (has_module) {
// most are in progress
result = vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS;
switch (status.state) {
case autotune_attitude_control_status_s::STATE_IDLE:
case autotune_attitude_control_status_s::STATE_INIT:
progress = 0;
break;
case autotune_attitude_control_status_s::STATE_ROLL:
case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
progress = 20;
break;
case autotune_attitude_control_status_s::STATE_PITCH:
case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
progress = 40;
break;
case autotune_attitude_control_status_s::STATE_YAW:
case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
progress = 60;
break;
case autotune_attitude_control_status_s::STATE_VERIFICATION:
progress = 80;
break;
case autotune_attitude_control_status_s::STATE_APPLY:
progress = 85;
break;
case autotune_attitude_control_status_s::STATE_TEST:
progress = 90;
break;
case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
progress = 95;
break;
case autotune_attitude_control_status_s::STATE_COMPLETE:
progress = 100;
// ack it properly with an ACCEPTED once we're done
result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
break;
case autotune_attitude_control_status_s::STATE_FAIL:
progress = 0;
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
break;
}
} else { } else {
result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED; acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
} }
send_ack = true; return;
} else { case MAV_CMD_DO_SET_ACTUATOR:
send_ack = false; if (_param_sys_ctrl_alloc.get()) {
_cmd_pub.publish(vehicle_command);
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { } else {
PX4_WARN("ignoring CMD with same SYS/COMP (%" PRIu8 "/%" PRIu8 ") ID", mavlink_system.sysid, mavlink_system.compid); // since we're only paying attention to 3 AUX outputs, the
return; // index should be 0, otherwise ignore the message
} int index = roundf(vehicle_command.param7);
if (cmd_mavlink.command == MAV_CMD_LOGGING_START) { if (index == 0) {
// check that we have enough bandwidth available: this is given by the configured logger topics actuator_controls_s actuator_controls{};
// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming // update with existing values to avoid changing unspecified controls
// on a radio link _actuator_controls_3_sub.update(&actuator_controls);
if (_mavlink->get_data_rate() < 5000) {
send_ack = true;
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming\t");
events::send<uint32_t>(events::ID("mavlink_log_not_enough_bw"), events::Log::Error,
"Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink->get_data_rate());
} else { bool updated = false;
// we already instanciate the streaming object, because at this point we know on which
// mavlink channel streaming was requested. But in fact it's possible that the logger is if (PX4_ISFINITE(vehicle_command.param1)) {
// not even running. The main mavlink thread takes care of this by waiting for an ack actuator_controls.control[5] = vehicle_command.param1;
// from the logger. updated = true;
_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); }
if (PX4_ISFINITE(vehicle_command.param2)) {
actuator_controls.control[6] = vehicle_command.param2;
updated = true;
}
if (PX4_ISFINITE(vehicle_command.param3)) {
actuator_controls.control[7] = vehicle_command.param3;
updated = true;
}
if (updated) {
actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls_pubs[3].publish(actuator_controls);
}
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED);
} }
} }
if (!send_ack) { return;
_cmd_pub.publish(vehicle_command);
}
}
if (send_ack) { case MAV_CMD_LOGGING_START:
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result, progress);
// check that we have enough bandwidth available: this is given by the configured logger topics
// and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming
// on a radio link
if (_mavlink->get_data_rate() < 5000) {
_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming\t");
events::send<uint32_t>(events::ID("mavlink_log_not_enough_bw"), events::Log::Error,
"Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink->get_data_rate());
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
} else {
// we already instanciate the streaming object, because at this point we know on which
// mavlink channel streaming was requested. But in fact it's possible that the logger is
// not even running. The main mavlink thread takes care of this by waiting for an ack
// from the logger.
_mavlink->try_start_ulog_streaming(msg->sysid, msg->compid);
}
return;
case MAV_CMD_DO_AUTOTUNE_ENABLE: {
autotune_attitude_control_status_s status{};
_autotune_attitude_control_status_sub.copy(&status);
bool has_module = (status.timestamp != 0);
// if not busy enable via the parameter
// do not check the return value of the uORB copy above because the module
// starts publishing only when MC_AT_START is set
if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.in_transition_mode) {
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED);
return;
} else {
param_t atune_start = PARAM_INVALID;
switch (vehicle_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
atune_start = param_find("FW_AT_START");
break;
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
atune_start = param_find("MC_AT_START");
break;
default:
break;
}
if (atune_start == PARAM_INVALID) {
has_module = false;
} else {
int32_t start = 1;
param_set(atune_start, &start);
}
}
}
if (has_module) {
// most are in progress
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS;
uint8_t progress = 0;
switch (status.state) {
case autotune_attitude_control_status_s::STATE_IDLE:
case autotune_attitude_control_status_s::STATE_INIT:
progress = 0;
break;
case autotune_attitude_control_status_s::STATE_ROLL:
case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
progress = 20;
break;
case autotune_attitude_control_status_s::STATE_PITCH:
case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
progress = 40;
break;
case autotune_attitude_control_status_s::STATE_YAW:
case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
progress = 60;
break;
case autotune_attitude_control_status_s::STATE_VERIFICATION:
progress = 80;
break;
case autotune_attitude_control_status_s::STATE_APPLY:
progress = 85;
break;
case autotune_attitude_control_status_s::STATE_TEST:
progress = 90;
break;
case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
progress = 95;
break;
case autotune_attitude_control_status_s::STATE_COMPLETE:
progress = 100;
// ack it properly with an ACCEPTED once we're done
result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
break;
case autotune_attitude_control_status_s::STATE_FAIL:
progress = 0;
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
break;
}
acknowledge(msg->sysid, msg->compid, command, result, progress);
} else {
acknowledge(msg->sysid, msg->compid, command, vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED);
}
}
return;
default:
// publish the command
_cmd_pub.publish(vehicle_command);
return;
} }
} }
src/modules/mavlink/mavlink_receiver.h
+4 -7
View File
@@ -139,12 +139,8 @@ private:
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress = 0); void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress = 0);
/** // Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t.
* Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t. void handle_message_command_both(mavlink_message_t *msg, uint16_t command, const vehicle_command_s &vehicle_command);
*/
template<class T>
void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
const vehicle_command_s &vehicle_command);
uint8_t handle_request_message_command(uint16_t message_id, float param2 = 0.0f, float param3 = 0.0f, uint8_t handle_request_message_command(uint16_t message_id, float param2 = 0.0f, float param3 = 0.0f,
float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
@@ -401,7 +397,8 @@ private:
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr, (ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr, (ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr (ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _param_sys_ctrl_alloc
); );
// Disallow copy construction and move assignment. // Disallow copy construction and move assignment.