Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: send warning to GCS on invalid GPS or angle targets

Browse Source 
This hopefully will help catch cases where the GCS relied on a bug in 4.2 (and earlier's) handling of MAV_CMD_DO_MOUNT_CONTROL
This commit is contained in:
Randy Mackay 2022-08-25 18:30:24 +09:00
parent 022d610519
commit 83c9a76260
2 changed files with 37 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -144,16 +144,32 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
set_mode(new_mode);
return MAV_RESULT_ACCEPTED;
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
// set body-frame target angles (in degrees) from mavlink message
// param1: pitch (in degrees)
// param2: roll in degrees
// param3: yaw in degrees
const float pitch_deg = packet.param1; // param1: pitch (in degrees)
const float roll_deg = packet.param2; // param2: roll in degrees
const float yaw_deg = packet.param3; // param3: yaw in degrees
// warn if angles are invalid to catch angles sent in centi-degrees
if ((fabsf(pitch_deg) > 90) || (fabsf(roll_deg) > 180) || (fabsf(yaw_deg) > 360)) {
send_warning_to_GCS("invalid angle targets");
return MAV_RESULT_FAILED;
}
set_angle_target(packet.param2, packet.param1, packet.param3, false);
return MAV_RESULT_ACCEPTED;
}
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
// 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)) {
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
@ -405,4 +421,16 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const
return get_angle_target_to_location(_target_sysid_location, angle_rad);
}
// sent warning to GCS. Warnings are throttled to at most once every 30 seconds
void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str)
{
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _last_warning_ms < 30000) {
return;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Mount: %s", warning_str);
_last_warning_ms = now_ms;
}
#endif // HAL_MOUNT_ENABLED

5
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -162,6 +162,9 @@ protected:
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t get_gimbal_device_flags() const;
// sent warning to GCS
void send_warning_to_GCS(const char* warning_str);
AP_Mount &_frontend; // reference to the front end which holds parameters
AP_Mount::mount_state &_state; // references to the parameters and state for this backend
uint8_t _instance; // this instance's number
@ -182,6 +185,8 @@ protected:
uint8_t _target_sysid; // sysid to track
Location _target_sysid_location;// sysid target location
bool _target_sysid_location_set;// true if _target_sysid has been set
uint32_t _last_warning_ms; // system time of last warning sent to GCS
};
#endif // HAL_MOUNT_ENABLED