aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: accept mount commands as command_int

Browse Source
This commit is contained in:
Peter Barker 2023-08-17 17:19:03 +10:00 committed by Peter Barker
parent c2be0a5767
commit 02d1eca49b
4 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Mount/AP_Mount.cpp
View File

@ -287,7 +287,7 @@ void AP_Mount::set_rate_target(uint8_t instance, float roll_degs, float pitch_de
backend->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock);
}
MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_long_t &packet)
MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int_t &packet)
{
auto *backend = get_primary();
if (backend == nullptr) {
@ -300,7 +300,7 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_lon
}
MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_t &packet)
MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t &packet)
{
auto *backend = get_primary();
if (backend == nullptr) {
@ -310,13 +310,13 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_
return backend->handle_command_do_mount_control(packet);
}
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet)
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet)
{
AP_Mount_Backend *backend;
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is
// 2nd gimbal, etc
const uint8_t instance = packet.param7;
const uint8_t instance = packet.z;
if (instance == 0) {
backend = get_primary();
} else {
@ -328,7 +328,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
}
// check flags for change to RETRACT
uint32_t flags = (uint32_t)packet.param5;
const uint32_t flags = packet.x;
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
backend->set_mode(MAV_MOUNT_MODE_RETRACT);
return MAV_RESULT_ACCEPTED;
@ -361,12 +361,12 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
}
// handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
AP_Mount_Backend *backend;
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
const uint8_t instance = packet.param7;
const uint8_t instance = packet.z;
if (instance == 0) {
backend = get_primary();
} else {
@ -499,7 +499,7 @@ void AP_Mount::handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_
}
}
MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
switch (packet.command) {
case MAV_CMD_DO_MOUNT_CONFIGURE:

10
libraries/AP_Mount/AP_Mount.h
View File

@ -178,7 +178,7 @@ public:
void set_target_sysid(uint8_t instance, uint8_t sysid);
// mavlink message handling:
MAV_RESULT handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
void handle_param_value(const mavlink_message_t &msg);
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
@ -278,10 +278,10 @@ private:
void handle_mount_configure(const mavlink_message_t &msg);
void handle_mount_control(const mavlink_message_t &msg);
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
void handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg);
void handle_global_position_int(const mavlink_message_t &msg);

18
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -249,9 +249,9 @@ void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packe
}
// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_command_long_t &packet)
MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_command_int_t &packet)
{
const MAV_MOUNT_MODE new_mode = (MAV_MOUNT_MODE)packet.param7;
const MAV_MOUNT_MODE new_mode = (MAV_MOUNT_MODE)packet.z;
// interpret message fields based on mode
switch (new_mode) {
@ -282,19 +282,19 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
case MAV_MOUNT_MODE_GPS_POINT: {
// set lat, lon, alt position targets from mavlink message
// warn if lat, lon appear to be in param1,2 instead of param5,6 as this indicates
// warn if lat, lon appear to be in param1,2 instead of param x,y as this indicates
// sender is relying on a bug in AP-4.2's (and earlier) handling of MAV_CMD_DO_MOUNT_CONTROL
if (!is_zero(packet.param1) && !is_zero(packet.param2) && is_zero(packet.param5) && is_zero(packet.param6)) {
if (!is_zero(packet.param1) && !is_zero(packet.param2) && packet.x == 0 && packet.y == 0) {
send_warning_to_GCS("GPS_POINT target invalid");
return MAV_RESULT_FAILED;
}
// param4: altitude in meters
// param5: latitude in degrees * 1E7
// param6: longitude in degrees * 1E7
// x: latitude in degrees * 1E7
// y: longitude in degrees * 1E7
const Location target_location {
(int32_t)packet.param5, // latitude in degrees * 1E7
(int32_t)packet.param6, // longitude in degrees * 1E7
packet.x, // latitude in degrees * 1E7
packet.y, // longitude in degrees * 1E7
(int32_t)packet.param4 * 100, // alt converted from meters to cm
Location::AltFrame::ABOVE_HOME
};
@ -310,7 +310,7 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
// sanity check param1 and param2 values
if ((packet.param1 < -3) || (packet.param1 > UINT8_MAX) || (packet.param2 < -3) || (packet.param2 > UINT8_MAX)) {

4
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -94,11 +94,11 @@ public:
void set_target_sysid(uint8_t sysid);
// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
// process MOUNT_CONFIGURE messages received from GCS. deprecated.
void handle_mount_configure(const mavlink_mount_configure_t &msg);