forked from Archive/PX4-Autopilot
mavlink: new args to request message, omit param1
We can omit param1 because it is already used for the message_id, so it's basically duplicate information and won't get used anywhere.
This commit is contained in:
parent
27b6c209e0
commit
b4cbd93b7c
|
@ -430,7 +430,11 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
|
|||
get_message_interval((int)roundf(cmd_mavlink.param1));
|
||||
|
||||
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
|
||||
result = handle_request_message_command(vehicle_command);
|
||||
|
||||
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 = {};
|
||||
|
@ -494,17 +498,16 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
|
|||
}
|
||||
}
|
||||
|
||||
uint8_t MavlinkReceiver::handle_request_message_command(const vehicle_command_s &vehicle_command)
|
||||
uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, float param2, float param3, float param4,
|
||||
float param5, float param6, float param7)
|
||||
{
|
||||
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
|
||||
bool stream_found = false;
|
||||
|
||||
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
for (const auto stream : _mavlink->get_streams()) {
|
||||
if (stream->get_id() == message_id) {
|
||||
stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
|
||||
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
|
||||
stream_found = stream->request_message(param2, param3, param4, param5, param6, param7);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -516,8 +519,7 @@ uint8_t MavlinkReceiver::handle_request_message_command(const vehicle_command_s
|
|||
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
|
||||
} else {
|
||||
bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
|
||||
vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
|
||||
bool message_sent = stream->request_message(param2, param3, param4, param5, param6, param7);
|
||||
|
||||
if (!message_sent) {
|
||||
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
|
|
|
@ -129,7 +129,9 @@ private:
|
|||
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(const vehicle_command_s &vehicle_command);
|
||||
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);
|
||||
|
||||
void handle_message(mavlink_message_t *msg);
|
||||
|
||||
|
|
|
@ -96,7 +96,7 @@ public:
|
|||
* This function is called in response to a MAV_CMD_REQUEST_MESSAGE command. The default implementation is to
|
||||
* just reset the counter to immediately send one message.
|
||||
*/
|
||||
virtual bool request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
|
||||
virtual bool request_message(float param2 = 0.0, float param3 = 0.0, float param4 = 0.0,
|
||||
float param5 = 0.0, float param6 = 0.0, float param7 = 0.0)
|
||||
{
|
||||
reset_last_sent();
|
||||
|
|
|
@ -36,7 +36,7 @@ public:
|
|||
return MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool request_message(float param1, float param2, float param3, float param4,
|
||||
bool request_message(float param2, float param3, float param4,
|
||||
float param5, float param6, float param7) override
|
||||
{
|
||||
return send(hrt_absolute_time());
|
||||
|
|
|
@ -38,7 +38,7 @@ public:
|
|||
return MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool request_message(float param1, float param2, float param3, float param4,
|
||||
bool request_message(float param2, float param3, float param4,
|
||||
float param5, float param6, float param7) override
|
||||
{
|
||||
return send(hrt_absolute_time());
|
||||
|
|
|
@ -36,7 +36,7 @@ public:
|
|||
return MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool request_message(float param1, float param2, float param3, float param4,
|
||||
bool request_message(float param2, float param3, float param4,
|
||||
float param5, float param6, float param7) override
|
||||
{
|
||||
return send(hrt_absolute_time());
|
||||
|
|
|
@ -45,7 +45,7 @@ public:
|
|||
return MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
bool request_message(float param1, float param2, float param3, float param4,
|
||||
bool request_message(float param2, float param3, float param4,
|
||||
float param5, float param6, float param7) override
|
||||
{
|
||||
storage_id = (int)roundf(param2);
|
||||
|
|
Loading…
Reference in New Issue