ardupilot/libraries/AP_Mount/AP_Mount.cpp

// -*- 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);
}