mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-08 02:57:16 +08:00
Peter BarkerandClaude Sonnet 4.6
14d09599de
Scripting backends (e.g. mount-djirs2-driver.lua) poll get_angle_target() and get_rate_target() rather than receiving pushed targets. Before this fix, get_angle_target() returned false for RETRACT, NEUTRAL, and LOCATION modes because it only checked target_type == ANGLE, even though send_target_to_gimbal() had already written the converted angle into mnt_target.angle_rad. Fix by calling send_target_to_gimbal() from AP_Mount_Scripting::update() (matching every other backend) and implementing send_target_angles() to stamp mnt_target.target_type = ANGLE after send_target_to_gimbal() has written the converted value into mnt_target.angle_rad. This makes get_angle_target() return the converted value without any changes to the base class. Declare NATIVE_ANGLES_AND_RATES_ONLY so that RATE targets are not converted to angles internally; the Lua script receives them via get_rate_target() and performs its own integration. send_target_rates() is a no-op because mnt_target.target_type stays RATE, which is all get_rate_target() checks. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
63 lines
2.0 KiB
C++
63 lines
2.0 KiB
C++
#include "AP_Mount_config.h"
|
|
|
|
#if HAL_MOUNT_SCRIPTING_ENABLED
|
|
|
|
#include "AP_Mount_Scripting.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define AP_MOUNT_SCRIPTING_TIMEOUT_MS 1000 // scripting mount becomes unhealthy after 1sec with no updates
|
|
|
|
#define AP_MOUNT_SCRIPTING_DEBUG 0
|
|
#define debug(fmt, args ...) do { if (AP_MOUNT_SCRIPTING_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0)
|
|
|
|
// update mount position - should be called periodically
|
|
void AP_Mount_Scripting::update()
|
|
{
|
|
AP_Mount_Backend::update();
|
|
|
|
update_mnt_target();
|
|
|
|
send_target_to_gimbal();
|
|
}
|
|
|
|
// return true if healthy
|
|
bool AP_Mount_Scripting::healthy() const
|
|
{
|
|
// healthy if scripting backend has updated actual angles recently
|
|
return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS);
|
|
}
|
|
|
|
// update mount's actual angles (to be called by script communicating with mount)
|
|
void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg)
|
|
{
|
|
last_update_ms = AP_HAL::millis();
|
|
current_angle_deg.x = roll_deg;
|
|
current_angle_deg.y = pitch_deg;
|
|
current_angle_deg.z = yaw_bf_deg;
|
|
}
|
|
|
|
// called by send_target_to_gimbal() after it has written the converted angle
|
|
// into mnt_target.angle_rad. Stamp the target_type as ANGLE so that the
|
|
// base-class get_angle_target() returns the value to the Lua script.
|
|
// This covers every non-ANGLE mode that converts to angles (RETRACT, NEUTRAL,
|
|
// LOCATION, and rate-to-angle if the backend were NATIVE_ANGLES_ONLY).
|
|
void AP_Mount_Scripting::send_target_angles(const MountAngleTarget &angle_rad)
|
|
{
|
|
mnt_target.target_type = MountTargetType::ANGLE;
|
|
}
|
|
|
|
// get attitude as a quaternion. returns true on success
|
|
bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat)
|
|
{
|
|
// construct quaternion
|
|
att_quat.from_euler(radians(current_angle_deg.x), radians(current_angle_deg.y), radians(current_angle_deg.z));
|
|
return true;
|
|
}
|
|
|
|
#endif // HAL_MOUNT_SCRIPTING_ENABLED
|