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

mavlink: fix SET_MESSAGE_INTERVAL parsing (#23796)

Browse Source 
This fixes the SITL tests that fail in CI because we catch NAN as non
zero after cast to int. To fix this I've added the check whether they
are finite at all.

The checks for param5 and param6 would be a bit trickier because they
can be int or float, so I have omitted them for now.
This commit is contained in:
Julian Oes
2024-10-11 09:03:22 +13:00
committed by GitHub
parent 9557a2da45
commit 01888a3085
2 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+15 -6
View File
@@ -593,8 +593,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
result = handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION);
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4,
(int)(vehicle_command.param5 + 0.5), (int)(vehicle_command.param6 + 0.5), (int)(vehicle_command.param7 + 0.5f))) {
if (set_message_interval(
(int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4, vehicle_command.param7)) {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
}
@@ -2274,15 +2274,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
}
int
MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, int param5, int param6,
int response_target)
MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, float param7)
{
if (msgId == MAVLINK_MSG_ID_HEARTBEAT) {
return PX4_ERROR;
}
if ((int)(param3 + 0.5f) || (int)(param4 + 0.5f) || param5 || param6 || response_target) {
// At least one of the unsupported params is non-zero
if (PX4_ISFINITE(param3) && (int)(param3 + 0.5f) != 0) {
PX4_ERR("SET_MESSAGE_INTERVAL requested param3 not supported.");
return PX4_ERROR;
}
if (PX4_ISFINITE(param4) && (int)(param4 + 0.5f) != 0) {
PX4_ERR("SET_MESSAGE_INTERVAL requested param4 not supported.");
return PX4_ERROR;
}
if (PX4_ISFINITE(param7) && (int)(param7 + 0.5f) != 0) {
PX4_ERR("SET_MESSAGE_INTERVAL response target not supported.");
return PX4_ERROR;
}
src/modules/mavlink/mavlink_receiver.h
+1 -2
View File
@@ -233,8 +233,7 @@ private:
*
* @return PX4_OK on success, PX4_ERROR on fail.
*/
int set_message_interval(int msgId, float interval, float param3 = 0.0f, float param4 = 0.0f, int param5 = 0,
int param6 = 0, int response_target = 0);
int set_message_interval(int msgId, float interval, float param3, float param4, float param7);
void get_message_interval(int msgId);
bool evaluate_target_ok(int command, int target_system, int target_component);