diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 313e329371..4cbed1b0e7 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -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 diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 1b006bbfd9..452e9d1f8a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -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