Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Mount/AP_Mount.cpp

727 lines
27 KiB
C++
Raw Blame History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_Mount.h>
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
#define MASK_TILT (1<<0)
#define MASK_ROLL (1<<1)
#define MASK_YAW (1<<2)
#define MASK_RETRACT (1<<3)
#if defined( __AVR_ATmega1280__ )
# define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount
# define MNT_RETRACT_OPTION DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings)
# define MNT_GPSPOINT_OPTION ENABLED // Point the mount to a GPS point defined via a mouse click in the Mission Planner GUI
# define MNT_STABILIZE_OPTION DISABLED // stabilize camera using frame attitude information
# define MNT_MOUNT2_OPTION DISABLED // second mount, can for example be used to keep an antenna pointed at the home position
#else
# define MNT_JSTICK_SPD_OPTION ENABLED // uses 844 bytes of memory
# define MNT_RETRACT_OPTION ENABLED // uses 244 bytes of memory
# define MNT_GPSPOINT_OPTION ENABLED // uses 580 bytes of memory
# define MNT_STABILIZE_OPTION ENABLED // uses 2424 bytes of memory
# define MNT_MOUNT2_OPTION ENABLED // uses 58 bytes of memory (must also be enabled in APM_Config.h)
#endif
const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
// @Param: MODE
// @DisplayName: Mount operation mode
// @Description: Camera or antenna mount operation mode
// @Values: 0:retract,1:neutral,2:MavLink_targeting,3:RC_targeting,4:GPS_point
// @User: Standard
AP_GROUPINFO("MODE", 0, AP_Mount, _mount_mode, MAV_MOUNT_MODE_RETRACT), // see MAV_MOUNT_MODE at ardupilotmega.h
#if MNT_RETRACT_OPTION == ENABLED
// @Param: RETRACT_X
// @DisplayName: Mount roll angle when in retracted position
// @Description: Mount roll angle when in retracted position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: RETRACT_Y
// @DisplayName: Mount tilt/pitch angle when in retracted position
// @Description: Mount tilt/pitch angle when in retracted position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: RETRACT_Z
// @DisplayName: Mount yaw/pan angle when in retracted position
// @Description: Mount yaw/pan angle when in retracted position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("RETRACT", 1, AP_Mount, _retract_angles, 0),
#endif
// @Param: NEUTRAL_X
// @DisplayName: Mount roll angle when in neutral position
// @Description: Mount roll angle when in neutral position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: NEUTRAL_Y
// @DisplayName: Mount tilt/pitch angle when in neutral position
// @Description: Mount tilt/pitch angle when in neutral position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
// @Param: NEUTRAL_Z
// @DisplayName: Mount pan/yaw angle when in neutral position
// @Description: Mount pan/yaw angle when in neutral position
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @User: Standard
AP_GROUPINFO("NEUTRAL", 2, AP_Mount, _neutral_angles, 0),
// @Param: CONTROL_X
// @DisplayName: Mount roll angle command from groundstation
// @Description: Mount roll angle when in MavLink or RC control operation mode
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @Param: CONTROL_Y
// @DisplayName: Mount tilt/pitch angle command from groundstation
// @Description: Mount tilt/pitch angle when in MavLink or RC control operation mode
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
// @Param: CONTROL_Z
// @DisplayName: Mount pan/yaw angle command from groundstation
// @Description: Mount pan/yaw angle when in MavLink or RC control operation mode
// @Units: Degrees
// @Range: -180.00 179.99
// @Increment: 1
AP_GROUPINFO("CONTROL", 3, AP_Mount, _control_angles, 0),
#if MNT_STABILIZE_OPTION == ENABLED
// @Param: STAB_ROLL
// @DisplayName: Stabilize mount's roll angle
// @Description: enable roll stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll, 0),
// @Param: STAB_TILT
// @DisplayName: Stabilize mount's pitch/tilt angle
// @Description: enable tilt/pitch stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("STAB_TILT", 5, AP_Mount, _stab_tilt, 0),
// @Param: STAB_PAN
// @DisplayName: Stabilize mount pan/yaw angle
// @Description: enable pan/yaw stabilisation relative to Earth
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("STAB_PAN", 6, AP_Mount, _stab_pan, 0),
#endif
// @Param: RC_IN_ROLL
// @DisplayName: roll RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control roll movements
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
// @User: Standard
AP_GROUPINFO("RC_IN_ROLL", 7, AP_Mount, _roll_rc_in, 0),
// @Param: ANGMIN_ROL
// @DisplayName: Minimum roll angle
// @Description: Minimum physical roll angular position of mount.
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMIN_ROL", 8, AP_Mount, _roll_angle_min, -4500),
// @Param: ANGMAX_ROL
// @DisplayName: Maximum roll angle
// @Description: Maximum physical roll angular position of the mount
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMAX_ROL", 9, AP_Mount, _roll_angle_max, 4500),
// @Param: RC_IN_TILT
// @DisplayName: tilt (pitch) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
// @User: Standard
AP_GROUPINFO("RC_IN_TILT", 10, AP_Mount, _tilt_rc_in, 0),
// @Param: ANGMIN_TIL
// @DisplayName: Minimum tilt angle
// @Description: Minimum physical tilt (pitch) angular position of mount.
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMIN_TIL", 11, AP_Mount, _tilt_angle_min, -4500),
// @Param: ANGMAX_TIL
// @DisplayName: Maximum tilt angle
// @Description: Maximum physical tilt (pitch) angular position of the mount
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMAX_TIL", 12, AP_Mount, _tilt_angle_max, 4500),
// @Param: RC_IN_PAN
// @DisplayName: pan (yaw) RC input channel
// @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements
// @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8
// @User: Standard
AP_GROUPINFO("RC_IN_PAN", 13, AP_Mount, _pan_rc_in, 0),
// @Param: ANGMIN_PAN
// @DisplayName: Minimum pan angle
// @Description: Minimum physical pan (yaw) angular position of mount.
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMIN_PAN", 14, AP_Mount, _pan_angle_min, -4500),
// @Param: ANGMAX_PAN
// @DisplayName: Maximum pan angle
// @Description: Maximum physical pan (yaw) angular position of the mount
// @Units: Centi-Degrees
// @Range: -18000 17999
// @Increment: 1
// @User: Standard
AP_GROUPINFO("ANGMAX_PAN", 15, AP_Mount, _pan_angle_max, 4500),
#if MNT_JSTICK_SPD_OPTION == ENABLED
// @Param: JSTICK_SPD
// @DisplayName: mount joystick speed
// @Description: 0 for position control, small for low speeds, 100 for max speed. A good general value is 10 which gives a movement speed of 3 degrees per second.
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("JSTICK_SPD", 16, AP_Mount, _joystick_speed, 0),
#endif
// @Param: LEAD_RLL
// @DisplayName: Roll stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate, compensating for servo delay. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("LEAD_RLL", 17, AP_Mount, _roll_stb_lead, 0.0f),
// @Param: LEAD_PTCH
// @DisplayName: Pitch stabilization lead time
// @Description: Causes the servo angle output to lead the current angle of the vehicle by some amount of time based on current angular rate. Increase until the servo is responsive but doesn't overshoot. Does nothing with pan stabilization enabled.
// @Units: Seconds
// @Range: 0.0 0.2
// @Increment: .005
// @User: Standard
AP_GROUPINFO("LEAD_PTCH", 18, AP_Mount, _pitch_stb_lead, 0.0f),
AP_GROUPEND
};
AP_Mount::AP_Mount(const struct Location *current_loc, const AP_AHRS &ahrs, uint8_t id) :
_ahrs(ahrs),
_roll_control_angle(0.0f),
_tilt_control_angle(0.0f),
_pan_control_angle(0.0f),
_roll_angle(0.0f),
_tilt_angle(0.0f),
_pan_angle(0.0f)
{
AP_Param::setup_object_defaults(this, var_info);
_current_loc = current_loc;
// default to zero angles
_retract_angles = Vector3f(0,0,0);
_neutral_angles = Vector3f(0,0,0);
_control_angles = Vector3f(0,0,0);
// default unknown mount type
_mount_type = k_unknown;
#if MNT_MOUNT2_OPTION == ENABLED
if (id == 0) {
#endif
_roll_idx = RC_Channel_aux::k_mount_roll;
_tilt_idx = RC_Channel_aux::k_mount_tilt;
_pan_idx = RC_Channel_aux::k_mount_pan;
_open_idx = RC_Channel_aux::k_mount_open;
#if MNT_MOUNT2_OPTION == ENABLED
} else {
_roll_idx = RC_Channel_aux::k_mount2_roll;
_tilt_idx = RC_Channel_aux::k_mount2_tilt;
_pan_idx = RC_Channel_aux::k_mount2_pan;
_open_idx = RC_Channel_aux::k_mount2_open;
}
#endif
}
/// Auto-detect the mount gimbal type depending on the functions assigned to the servos
void
AP_Mount::update_mount_type()
{
bool have_roll, have_tilt, have_pan;
have_roll = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_roll) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_roll);
have_tilt = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_tilt) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_tilt);
have_pan = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_pan) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_pan);
mount_axis_mask = 0;
if (have_pan)
mount_axis_mask |= MASK_YAW;
if (have_tilt)
mount_axis_mask |= MASK_TILT;
if (have_roll)
mount_axis_mask |= MASK_ROLL;
if (have_pan && have_tilt && !have_roll)
_mount_type = k_pan_tilt;
if (!have_pan && have_tilt && have_roll)
_mount_type = k_tilt_roll;
if (have_pan && have_tilt && have_roll)
_mount_type = k_pan_tilt_roll;
}
/// sets the servo angles for retraction, note angles are in degrees
void AP_Mount::set_retract_angles(float roll, float tilt, float pan)
{
_retract_angles = Vector3f(roll, tilt, pan);
}
//sets the servo angles for neutral, note angles are in degrees
void AP_Mount::set_neutral_angles(float roll, float tilt, float pan)
{
_neutral_angles = Vector3f(roll, tilt, pan);
}
/// sets the servo angles for MAVLink, note angles are in degrees
void AP_Mount::set_control_angles(float roll, float tilt, float pan)
{
_control_angles = Vector3f(roll, tilt, pan);
}
/// used to tell the mount to track GPS location
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
{
_target_GPS_location=targetGPSLocation;
}
/// This one should be called periodically
void AP_Mount::update_mount_position()
{
#if MNT_RETRACT_OPTION == ENABLED
static bool mount_open = 0; // 0 is closed
#endif
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
{
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
Vector3f vec = _retract_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
#endif
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
Vector3f vec = _control_angles.get();
_roll_control_angle = radians(vec.x);
_tilt_control_angle = radians(vec.y);
_pan_control_angle = radians(vec.z);
stabilize();
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
#define rc_ch(i) RC_Channel::rc_channel(i-1)
#if MNT_JSTICK_SPD_OPTION == ENABLED
if (_joystick_speed) { // for spring loaded joysticks
// allow pilot speed position input to come directly from an RC_Channel
if (_roll_rc_in && rc_ch(_roll_rc_in)) {
_roll_control_angle += rc_ch(_roll_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
if (_roll_control_angle < radians(_roll_angle_min*0.01f)) _roll_control_angle = radians(_roll_angle_min*0.01f);
if (_roll_control_angle > radians(_roll_angle_max*0.01f)) _roll_control_angle = radians(_roll_angle_max*0.01f);
}
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
_tilt_control_angle += rc_ch(_tilt_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
if (_tilt_control_angle < radians(_tilt_angle_min*0.01f)) _tilt_control_angle = radians(_tilt_angle_min*0.01f);
if (_tilt_control_angle > radians(_tilt_angle_max*0.01f)) _tilt_control_angle = radians(_tilt_angle_max*0.01f);
}
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
_pan_control_angle += rc_ch(_pan_rc_in)->norm_input_dz() * 0.0001f * _joystick_speed;
if (_pan_control_angle < radians(_pan_angle_min*0.01f)) _pan_control_angle = radians(_pan_angle_min*0.01f);
if (_pan_control_angle > radians(_pan_angle_max*0.01f)) _pan_control_angle = radians(_pan_angle_max*0.01f);
}
} else {
#endif
// allow pilot position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch(_roll_rc_in))) {
_roll_control_angle = angle_input_rad(rc_ch(_roll_rc_in), _roll_angle_min, _roll_angle_max);
}
if (_tilt_rc_in && (rc_ch(_tilt_rc_in))) {
_tilt_control_angle = angle_input_rad(rc_ch(_tilt_rc_in), _tilt_angle_min, _tilt_angle_max);
}
if (_pan_rc_in && (rc_ch(_pan_rc_in))) {
_pan_control_angle = angle_input_rad(rc_ch(_pan_rc_in), _pan_angle_min, _pan_angle_max);
}
#if MNT_JSTICK_SPD_OPTION == ENABLED
}
#endif
stabilize();
break;
}
#if MNT_GPSPOINT_OPTION == ENABLED
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_GPS_target_angle(&_target_GPS_location);
stabilize();
}
break;
}
#endif
default:
//do nothing
break;
}
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" into the fuselage with a fourth servo
bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1;
if (mount_open != mount_open_new) {
mount_open = mount_open_new;
move_servo(_open_idx, mount_open_new, 0, 1);
}
#endif
// write the results to the servos
move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1f, _roll_angle_max*0.1f);
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1f, _tilt_angle_max*0.1f);
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1f, _pan_angle_max*0.1f);
}
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
{
_mount_mode = (int8_t)mode;
}
/// Change the configuration of the mount
/// triggered by a MavLink packet.
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
mavlink_msg_mount_configure_decode(msg, &packet);
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
_stab_roll = packet.stab_roll;
_stab_tilt = packet.stab_pitch;
_stab_pan = packet.stab_yaw;
}
/// Control the mount (depends on the previously set mount configuration)
/// triggered by a MavLink packet.
void AP_Mount::control_msg(mavlink_message_t *msg)
{
__mavlink_mount_control_t packet;
mavlink_msg_mount_control_decode(msg, &packet);
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
{
#if MNT_RETRACT_OPTION == ENABLED
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
set_retract_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
if (packet.save_position)
{
_retract_angles.save();
}
break;
#endif
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
set_neutral_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
if (packet.save_position)
{
_neutral_angles.save();
}
break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
set_control_angles(packet.input_b*0.01f, packet.input_a*0.01f, packet.input_c*0.01f);
break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
// also reset control angles, in radians. This allows a
// MOUNT_CONTROL message to be used to reset the mount to zero
// offset when in joystick speed relative mode
_roll_control_angle = radians(_roll_angle);
_tilt_control_angle = radians(_tilt_angle);
_pan_control_angle = radians(_pan_angle);
}
break;
#if MNT_GPSPOINT_OPTION == ENABLED
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
Location targetGPSLocation;
targetGPSLocation.lat = packet.input_a;
targetGPSLocation.lng = packet.input_b;
targetGPSLocation.alt = packet.input_c;
set_GPS_target_location(targetGPSLocation);
break;
#endif
case MAV_MOUNT_MODE_ENUM_END:
break;
default:
// do nothing
break;
}
}
/// Return mount status information
void AP_Mount::status_msg(mavlink_channel_t chan)
{
mavlink_msg_mount_status_send(chan,0,0, _tilt_angle*100, _roll_angle*100, _pan_angle*100);
}
/// Set mount point/region of interest, triggered by mission script commands
void AP_Mount::set_roi_cmd(const struct Location *target_loc)
{
#if MNT_GPSPOINT_OPTION == ENABLED
// set the target gps location
_target_GPS_location = *target_loc;
// set the mode to GPS tracking mode
set_mode(MAV_MOUNT_MODE_GPS_POINT);
#endif
}
/// Set mount configuration, triggered by mission script commands
void AP_Mount::configure_cmd()
{
// TODO get the information out of the mission command and use it
}
/// Control the mount (depends on the previously set mount configuration), triggered by mission script commands
void AP_Mount::control_cmd()
{
// TODO get the information out of the mission command and use it
}
/// returns the angle (degrees*100) that the RC_Channel input is receiving
int32_t
AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{
return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min);
}
/// returns the angle (radians) that the RC_Channel input is receiving
float
AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{
return radians(angle_input(rc, angle_min, angle_max)*0.01f);
}
void
AP_Mount::calc_GPS_target_angle(const struct Location *target)
{
float GPS_vector_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f;
float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f;
float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
if (mount_axis_mask & MASK_TILT) {
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
} else {
_tilt_control_angle = 0;
}
if (mount_axis_mask & MASK_YAW) {
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
} else {
_pan_control_angle = 0;
}
}
/// Stabilizes mount relative to the Earth's frame
/// Inputs:
/// _roll_control_angle desired roll angle in radians,
/// _tilt_control_angle desired tilt/pitch angle in radians,
/// _pan_control_angle desired pan/yaw angle in radians
/// Outputs:
/// _roll_angle stabilized roll angle in degrees,
/// _tilt_angle stabilized tilt/pitch angle in degrees,
/// _pan_angle stabilized pan/yaw angle in degrees
void
AP_Mount::stabilize()
{
#if MNT_STABILIZE_OPTION == ENABLED
// only do the full 3D frame transform if we are doing pan control
if (_stab_pan) {
Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs
Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input.
Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
m = _ahrs.get_dcm_matrix();
m.transpose();
cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle);
gimbal_target = m * cam;
gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle);
_roll_angle = _stab_roll ? degrees(_roll_angle) : degrees(_roll_control_angle);
_tilt_angle = _stab_tilt ? degrees(_tilt_angle) : degrees(_tilt_control_angle);
_pan_angle = degrees(_pan_angle);
} else {
// otherwise base mount roll and tilt on the ahrs
// roll/tilt attitude, plus any requested angle
_roll_angle = degrees(_roll_control_angle);
_tilt_angle = degrees(_tilt_control_angle);
_pan_angle = degrees(_pan_control_angle);
if (_stab_roll) {
_roll_angle -= degrees(_ahrs.roll);
}
if (_stab_tilt) {
_tilt_angle -= degrees(_ahrs.pitch);
}
// Add lead filter.
const Vector3f &gyro = _ahrs.get_gyro();
if (_stab_roll && _roll_stb_lead != 0.0f && fabsf(_ahrs.pitch) < M_PI/3.0f) {
// Compute rate of change of euler roll angle
float roll_rate = gyro.x + (_ahrs.sin_pitch() / _ahrs.cos_pitch()) * (gyro.y * _ahrs.sin_roll() + gyro.z * _ahrs.cos_roll());
_roll_angle -= degrees(roll_rate) * _roll_stb_lead;
}
if (_stab_tilt && _pitch_stb_lead != 0.0f) {
// Compute rate of change of euler pitch angle
float pitch_rate = _ahrs.cos_pitch() * gyro.y - _ahrs.sin_roll() * gyro.z;
_tilt_angle -= degrees(pitch_rate) * _pitch_stb_lead;
}
}
#endif
}
/*
* /// For testing and development. Called in the medium loop.
* void
* AP_Mount::debug_output()
* { Serial3.print("current - ");
* Serial3.print("lat ");
* Serial3.print(_current_loc->lat);
* Serial3.print(",lon ");
* Serial3.print(_current_loc->lng);
* Serial3.print(",alt ");
* Serial3.println(_current_loc->alt);
*
* Serial3.print("gps - ");
* Serial3.print("lat ");
* Serial3.print(_gps->latitude);
* Serial3.print(",lon ");
* Serial3.print(_gps->longitude);
* Serial3.print(",alt ");
* Serial3.print(_gps->altitude);
* Serial3.println();
*
* Serial3.print("target - ");
* Serial3.print("lat ");
* Serial3.print(_target_GPS_location.lat);
* Serial3.print(",lon ");
* Serial3.print(_target_GPS_location.lng);
* Serial3.print(",alt ");
* Serial3.print(_target_GPS_location.alt);
* Serial3.print(" hdg to targ ");
* Serial3.print(degrees(_pan_control_angle));
* Serial3.println();
* }
*/
/// saturate to the closest angle limit if outside of [min max] angle interval
/// input angle is in degrees * 10
int16_t
AP_Mount::closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max)
{
// Make sure the angle lies in the interval [-180 .. 180[ degrees
while (angle < -1800) angle += 3600;
while (angle >= 1800) angle -= 3600;
// Make sure the angle limits lie in the interval [-180 .. 180[ degrees
while (*angle_min < -1800) *angle_min += 3600;
while (*angle_min >= 1800) *angle_min -= 3600;
while (*angle_max < -1800) *angle_max += 3600;
while (*angle_max >= 1800) *angle_max -= 3600;
// TODO call this function somehow, otherwise this will never work
//set_range(min, max);
// If the angle is outside servo limits, saturate the angle to the closest limit
// On a circle the closest angular position must be carefully calculated to account for wrap-around
if ((angle < *angle_min) && (angle > *angle_max)) {
// angle error if min limit is used
int16_t err_min = *angle_min - angle + (angle<*angle_min ? 0 : 3600); // add 360 degrees if on the "wrong side"
// angle error if max limit is used
int16_t err_max = angle - *angle_max + (angle>*angle_max ? 0 : 3600); // add 360 degrees if on the "wrong side"
angle = err_min<err_max ? *angle_min : *angle_max;
}
return angle;
}
/// all angles are degrees * 10 units
void
AP_Mount::move_servo(uint8_t function_idx, int16_t angle, int16_t angle_min, int16_t angle_max)
{
// saturate to the closest angle limit if outside of [min max] angle interval
int16_t servo_out = closest_limit(angle, &angle_min, &angle_max);
RC_Channel_aux::move_servo((RC_Channel_aux::Aux_servo_function_t)function_idx, servo_out, angle_min, angle_max);
}
Reference in New Issue View Git Blame Copy Permalink