AP_Mount: Allow using any RC channel to control any of the mount axes.
This has the added benefit of saving 60 bytes and simplifying Mission Planner gui. Moved some code from RC_Channel_aux to AP_Mount class The servos get written by the update_mount_position() function, this simplifies main() PS: The beauty of using libraries: I did not have to touch a single line of ArduPlane's code!
This commit is contained in:
Amilcar Lucas
parent
b8c7b8a786
commit
107155fa1a
@ -1262,31 +1262,8 @@ static void fifty_hz_loop()
|
||||
#endif
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
static bool mount_initialised = false;
|
||||
|
||||
// do camera mount initialisation. TO-DO: move to AP_Mount library or init_ardupilot
|
||||
if( mount_initialised == false ) {
|
||||
mount_initialised = true;
|
||||
|
||||
// set-up channel 6 to control pitch
|
||||
camera_mount.set_manual_rc_channel( &g.rc_6 );
|
||||
|
||||
}
|
||||
|
||||
// update pilot's commands to mount
|
||||
if( g.radio_tuning == 0 ) {
|
||||
camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_PITCH );
|
||||
}else{
|
||||
camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED );
|
||||
}
|
||||
|
||||
// update camera mount's position
|
||||
camera_mount.update_mount_position();
|
||||
|
||||
// move camera servos. TO-DO: move this to AP_Mount library
|
||||
g.rc_camera_roll.output();
|
||||
g.rc_camera_pitch.output();
|
||||
g.rc_camera_yaw.output();
|
||||
#endif
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
|
||||
@ -47,23 +47,109 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll),
|
||||
|
||||
// @Param: STAB_PITCH
|
||||
// @DisplayName: Stabilize mount pitch
|
||||
// @Description: enable pitch/tilt stabilisation relative to Earth
|
||||
// @Param: STAB_TILT
|
||||
// @DisplayName: Stabilize mount tilt
|
||||
// @Description: enable tilt (pitch) stabilisation relative to Earth
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("STAB_PITCH", 5, AP_Mount, _stab_pitch),
|
||||
AP_GROUPINFO("STAB_TILT", 5, AP_Mount, _stab_tilt),
|
||||
|
||||
// @Param: STAB_YAW
|
||||
// @DisplayName: Stabilize mount yaw
|
||||
// @Description: enable yaw/pan stabilisation relative to Earth
|
||||
// @Param: STAB_PAN
|
||||
// @DisplayName: Stabilize mount pan
|
||||
// @Description: enable pan (yaw) stabilisation relative to Earth
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("STAB_YAW", 6, AP_Mount, _stab_yaw),
|
||||
AP_GROUPEND
|
||||
AP_GROUPINFO("STAB_PAN", 6, AP_Mount, _stab_pan),
|
||||
|
||||
// @Param: ROLL_RC_IN
|
||||
// @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("ROLL_RC_IN", 7, AP_Mount, _roll_rc_in),
|
||||
|
||||
// @Param: ROLL_ANGLE_MIN
|
||||
// @DisplayName: Minimum roll angle
|
||||
// @Description: Minimum physical roll angular position of mount.
|
||||
// @Units: centi-Degrees
|
||||
// @Range: -18000 17999
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ROLL_ANGMIN", 8, AP_Mount, _roll_angle_min),
|
||||
|
||||
// @Param: ROLL_ANGLE_MAX
|
||||
// @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("ROLL_ANGMAX", 9, AP_Mount, _roll_angle_max),
|
||||
|
||||
// @Param: TILT_RC_IN
|
||||
// @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("TILT_RC_IN", 10, AP_Mount, _tilt_rc_in),
|
||||
|
||||
// @Param: TILT_ANGLE_MIN
|
||||
// @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("TILT_ANGMIN", 11, AP_Mount, _tilt_angle_min),
|
||||
|
||||
// @Param: TILT_ANGLE_MAX
|
||||
// @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("TILT_ANGMAX", 12, AP_Mount, _tilt_angle_max),
|
||||
|
||||
// @Param: PAN_RC_IN
|
||||
// @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("PAN_RC_IN", 13, AP_Mount, _pan_rc_in),
|
||||
|
||||
// @Param: PAN_ANGLE_MIN
|
||||
// @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("PAN_ANGMIN", 14, AP_Mount, _pan_angle_min),
|
||||
|
||||
// @Param: PAN_ANGLE_MAX
|
||||
// @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("PAN_ANGMAX", 15, AP_Mount, _pan_angle_max),
|
||||
/*
|
||||
Must be commented out because the framework does not support more than 16 parameters
|
||||
// @Param: JOYSTICK_SPEED
|
||||
// @DisplayName: mount joystick speed
|
||||
// @Description: 0 for position control, small for low speeds, 10 for max speed
|
||||
// @Range: 0 10
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("JOYSTICK_SPEED", 16, AP_Mount, _joystick_speed),
|
||||
*/
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
extern RC_Channel* rc_ch[NUM_CHANNELS];
|
||||
|
||||
AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs):
|
||||
_gps(gps)
|
||||
@ -79,56 +165,65 @@ _gps(gps)
|
||||
_neutral_angles = Vector3f(0,0,0);
|
||||
_control_angles = Vector3f(0,0,0);
|
||||
|
||||
// default mount type to roll/pitch (which is the most common for copter)
|
||||
_mount_type = k_tilt_roll;
|
||||
// default unknown mount type
|
||||
_mount_type = k_unknown;
|
||||
|
||||
// default manual rc channel to disabled
|
||||
_manual_rc = NULL;
|
||||
_manual_rc_function = AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED;
|
||||
_pan_rc_in = 0;
|
||||
_tilt_rc_in= 0;
|
||||
_roll_rc_in= 0;
|
||||
|
||||
_pan_angle_min = -4500; // assume -45 degrees min deflection
|
||||
_pan_angle_max = 4500; // assume 45 degrees max deflection
|
||||
_tilt_angle_min = -4500; // assume -45 degrees min deflection
|
||||
_tilt_angle_max = 4500; // assume 45 degrees max deflection
|
||||
_roll_angle_min = -4500; // assume -45 degrees min deflection
|
||||
_roll_angle_max = 4500; // assume 45 degrees max deflection
|
||||
}
|
||||
|
||||
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
void AP_Mount::update_mount_type()
|
||||
/// Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
void
|
||||
AP_Mount::update_mount_type()
|
||||
{
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != NULL))
|
||||
{
|
||||
_mount_type = k_pan_tilt;
|
||||
}
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL))
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] == NULL))
|
||||
{
|
||||
_mount_type = k_tilt_roll;
|
||||
}
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != NULL))
|
||||
{
|
||||
_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 pitch, float yaw)
|
||||
/// 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, pitch, yaw);
|
||||
_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 pitch, float yaw)
|
||||
void AP_Mount::set_neutral_angles(float roll, float tilt, float pan)
|
||||
{
|
||||
_neutral_angles = Vector3f(roll, pitch, yaw);
|
||||
_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 pitch, float yaw)
|
||||
/// 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, pitch, yaw);
|
||||
_control_angles = Vector3f(roll, tilt, pan);
|
||||
}
|
||||
|
||||
// used to tell the mount to track GPS location
|
||||
/// 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
|
||||
/// This one should be called periodically
|
||||
void AP_Mount::update_mount_position()
|
||||
{
|
||||
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
|
||||
@ -138,8 +233,8 @@ void AP_Mount::update_mount_position()
|
||||
{
|
||||
Vector3f vec = _retract_angles.get();
|
||||
_roll_angle = vec.x;
|
||||
_pitch_angle = vec.y;
|
||||
_yaw_angle = vec.z;
|
||||
_tilt_angle = vec.y;
|
||||
_pan_angle = vec.z;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -148,8 +243,8 @@ void AP_Mount::update_mount_position()
|
||||
{
|
||||
Vector3f vec = _neutral_angles.get();
|
||||
_roll_angle = vec.x;
|
||||
_pitch_angle = vec.y;
|
||||
_yaw_angle = vec.z;
|
||||
_tilt_angle = vec.y;
|
||||
_pan_angle = vec.z;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -158,8 +253,8 @@ void AP_Mount::update_mount_position()
|
||||
{
|
||||
Vector3f vec = _control_angles.get();
|
||||
_roll_control_angle = radians(vec.x);
|
||||
_pitch_control_angle = radians(vec.y);
|
||||
_yaw_control_angle = radians(vec.z);
|
||||
_tilt_control_angle = radians(vec.y);
|
||||
_pan_control_angle = radians(vec.z);
|
||||
stabilize();
|
||||
break;
|
||||
}
|
||||
@ -167,40 +262,38 @@ void AP_Mount::update_mount_position()
|
||||
// RC radio manual angle control, but with stabilization from the AHRS
|
||||
case MAV_MOUNT_MODE_RC_TARGETING:
|
||||
{
|
||||
//#define MNT_SPRING_LOADED_JOYSTICK
|
||||
#ifdef MNT_SPRING_LOADED_JOYSTICK
|
||||
// rc_input() takes degrees * 100 units
|
||||
G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100);
|
||||
G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100);
|
||||
G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100);
|
||||
#else
|
||||
// allow pilot input to come directly from an RC_Channel
|
||||
if( _manual_rc != NULL ) {
|
||||
float manual_angle = radians((float)(4500 - _manual_rc->control_in ) / 100.0);
|
||||
switch( _manual_rc_function ) {
|
||||
case AP_MOUNT_MANUAL_RC_FUNCTION_ROLL:
|
||||
_roll_control_angle = manual_angle;
|
||||
break;
|
||||
case AP_MOUNT_MANUAL_RC_FUNCTION_PITCH:
|
||||
_pitch_control_angle = manual_angle;
|
||||
break;
|
||||
case AP_MOUNT_MANUAL_RC_FUNCTION_YAW:
|
||||
_yaw_control_angle = manual_angle;
|
||||
break;
|
||||
default:
|
||||
// do nothing
|
||||
break;
|
||||
/* 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-1])) {
|
||||
//_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed;
|
||||
_roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
|
||||
if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01);
|
||||
if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01);
|
||||
}
|
||||
}else{
|
||||
// take pilot's input from RC_Channel_aux
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_roll])
|
||||
_roll_control_angle = g_rc_function[RC_Channel_aux::k_mount_roll]->angle_input_rad();
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
|
||||
_pitch_control_angle = g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_input_rad();
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
|
||||
_yaw_control_angle = g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_input_rad();
|
||||
}
|
||||
#endif
|
||||
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
|
||||
//_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed;
|
||||
_tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
|
||||
if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01);
|
||||
if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01);
|
||||
}
|
||||
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
|
||||
//_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed;
|
||||
_pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
|
||||
if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01);
|
||||
if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01);
|
||||
}
|
||||
} else {
|
||||
*/ // allow pilot position input to come directly from an RC_Channel
|
||||
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
|
||||
_roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max);
|
||||
}
|
||||
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
|
||||
_tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max);
|
||||
}
|
||||
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
|
||||
_pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max);
|
||||
}
|
||||
// }
|
||||
stabilize();
|
||||
break;
|
||||
}
|
||||
@ -220,10 +313,9 @@ void AP_Mount::update_mount_position()
|
||||
}
|
||||
|
||||
// write the results to the servos
|
||||
// closest_limit() takes degrees * 10 units
|
||||
G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle*10);
|
||||
G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle*10);
|
||||
G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle*10);
|
||||
move_servo(g_rc_function[RC_Channel_aux::k_mount_roll], _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1);
|
||||
move_servo(g_rc_function[RC_Channel_aux::k_mount_tilt], _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1);
|
||||
move_servo(g_rc_function[RC_Channel_aux::k_mount_pan ], _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1);
|
||||
}
|
||||
|
||||
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
|
||||
@ -231,8 +323,8 @@ 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.
|
||||
/// 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;
|
||||
@ -242,13 +334,13 @@ void AP_Mount::configure_msg(mavlink_message_t* msg)
|
||||
return;
|
||||
}
|
||||
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
|
||||
_stab_pitch = packet.stab_pitch;
|
||||
_stab_roll = packet.stab_roll;
|
||||
_stab_yaw = packet.stab_yaw;
|
||||
_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.
|
||||
/// 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;
|
||||
@ -284,8 +376,8 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
|
||||
{
|
||||
Vector3f vec = _neutral_angles.get();
|
||||
_roll_angle = vec.x;
|
||||
_pitch_angle = vec.y;
|
||||
_yaw_angle = vec.z;
|
||||
_tilt_angle = vec.y;
|
||||
_pan_angle = vec.z;
|
||||
}
|
||||
break;
|
||||
|
||||
@ -306,8 +398,8 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
// Return mount status information (depends on the previously set mount configuration)
|
||||
// triggered by a MavLink packet.
|
||||
/// Return mount status information (depends on the previously set mount configuration)
|
||||
/// triggered by a MavLink packet.
|
||||
void AP_Mount::status_msg(mavlink_message_t *msg)
|
||||
{
|
||||
__mavlink_mount_status_t packet;
|
||||
@ -324,8 +416,8 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
|
||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
|
||||
case MAV_MOUNT_MODE_RC_TARGETING: // neutral position and start RC Roll,Pitch,Yaw control with stabilization
|
||||
packet.pointing_b = _roll_angle*100; ///< degrees*100
|
||||
packet.pointing_a = _pitch_angle*100; ///< degrees*100
|
||||
packet.pointing_c = _yaw_angle*100; ///< degrees*100
|
||||
packet.pointing_a = _tilt_angle*100; ///< degrees*100
|
||||
packet.pointing_c = _pan_angle*100; ///< degrees*100
|
||||
break;
|
||||
case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt
|
||||
packet.pointing_a = _target_GPS_location.lat; ///< latitude
|
||||
@ -342,7 +434,7 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
|
||||
packet.pointing_a, packet.pointing_b, packet.pointing_c);
|
||||
}
|
||||
|
||||
// Set mount point/region of interest, triggered by mission script commands
|
||||
/// Set mount point/region of interest, triggered by mission script commands
|
||||
void AP_Mount::set_roi_cmd(struct Location *target_loc)
|
||||
{
|
||||
// set the target gps location
|
||||
@ -352,105 +444,91 @@ void AP_Mount::set_roi_cmd(struct Location *target_loc)
|
||||
set_mode(MAV_MOUNT_MODE_GPS_POINT);
|
||||
}
|
||||
|
||||
// Set mount configuration, triggered by mission script commands
|
||||
/// 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
|
||||
/// 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.01);
|
||||
}
|
||||
|
||||
void
|
||||
AP_Mount::calc_GPS_target_angle(struct Location *target)
|
||||
{
|
||||
float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)/(t7*2.0)))*.01113195;
|
||||
float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.01113195;
|
||||
float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
|
||||
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.0*sqrt(GPS_vector_x*GPS_vector_x + GPS_vector_y*GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
|
||||
_roll_control_angle = 0;
|
||||
_pitch_control_angle = atan2(GPS_vector_z, target_distance);
|
||||
_yaw_control_angle = atan2(GPS_vector_x, GPS_vector_y);
|
||||
/*
|
||||
// Converts +/- 180 into 0-360.
|
||||
if(_yaw_control_angle<0){
|
||||
_yaw_control_angle += 2*M_PI;
|
||||
}
|
||||
*/
|
||||
_roll_control_angle = 0;
|
||||
_tilt_control_angle = atan2(GPS_vector_z, target_distance);
|
||||
_pan_control_angle = atan2(GPS_vector_x, GPS_vector_y);
|
||||
}
|
||||
|
||||
/// Stabilizes mount relative to the Earth's frame
|
||||
/// Inputs:
|
||||
/// _roll_control_angle desired roll angle in radians,
|
||||
/// _pitch_control_angle desired pitch/tilt angle in radians,
|
||||
/// _yaw_control_angle desired yaw/pan 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,
|
||||
/// _pitch_angle stabilized pitch/tilt angle in degrees,
|
||||
/// _yaw_angle stabilized yaw/pan angle in degrees
|
||||
/// _tilt_angle stabilized tilt/pitch angle in degrees,
|
||||
/// _pan_angle stabilized pan/yaw angle in degrees
|
||||
void
|
||||
AP_Mount::stabilize()
|
||||
{
|
||||
if (_ahrs) {
|
||||
// only do the full 3D frame transform if we are doing yaw control
|
||||
if (_stab_yaw) {
|
||||
// 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, _pitch_control_angle, _yaw_control_angle);
|
||||
cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle);
|
||||
gimbal_target = m * cam;
|
||||
gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle);
|
||||
gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle);
|
||||
_roll_angle = _stab_roll?degrees(_roll_angle):degrees(_roll_control_angle);
|
||||
_pitch_angle = _stab_pitch?degrees(_pitch_angle):degrees(_pitch_control_angle);
|
||||
_yaw_angle = degrees(_yaw_angle);
|
||||
_tilt_angle = _stab_tilt?degrees(_tilt_angle):degrees(_tilt_control_angle);
|
||||
_pan_angle = degrees(_pan_angle);
|
||||
} else {
|
||||
// otherwise base mount roll and pitch on the ahrs
|
||||
// roll/pitch attitude, plus any requested angle
|
||||
// otherwise base mount roll and tilt on the ahrs
|
||||
// roll/tilt attitude, plus any requested angle
|
||||
_roll_angle = degrees(_roll_control_angle);
|
||||
_pitch_angle = degrees(_pitch_control_angle);
|
||||
_yaw_angle = degrees(_yaw_control_angle);
|
||||
_tilt_angle = degrees(_tilt_control_angle);
|
||||
_pan_angle = degrees(_pan_control_angle);
|
||||
if (_stab_roll) {
|
||||
_roll_angle -= degrees(_ahrs->roll);
|
||||
}
|
||||
if (_stab_pitch) {
|
||||
_pitch_angle -= degrees(_ahrs->pitch);
|
||||
if (_stab_tilt) {
|
||||
_tilt_angle -= degrees(_ahrs->pitch);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
_roll_angle = degrees(_roll_control_angle);
|
||||
_pitch_angle = degrees(_pitch_control_angle);
|
||||
_yaw_angle = degrees(_yaw_control_angle);
|
||||
_tilt_angle = degrees(_tilt_control_angle);
|
||||
_pan_angle = degrees(_pan_control_angle);
|
||||
}
|
||||
}
|
||||
|
||||
// set_manual_rc_channel - define which RC_Channel is to be used for manual control
|
||||
void
|
||||
AP_Mount::set_manual_rc_channel(RC_Channel* rc)
|
||||
{
|
||||
_manual_rc = rc;
|
||||
}
|
||||
|
||||
// set_manual_rc_channel_function - set whether manual rc channel is disabled, controls roll (1), pitch (2) or yaw (3).
|
||||
void
|
||||
AP_Mount::set_manual_rc_channel_function(int8_t fn)
|
||||
{
|
||||
// update scaler if the function has changed
|
||||
if( _manual_rc_function != fn ) {
|
||||
_manual_rc_function = fn;
|
||||
|
||||
// ensure pilot's input appears in the range 0 ~ 9000 ( 90 degrees * 100 ). We will subtract 45 degrees later to make the range +-45 degrees
|
||||
if( _manual_rc != NULL && fn != AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED ) {
|
||||
_manual_rc->set_range( 0, 9000 );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// For testing and development. Called in the medium loop.
|
||||
/*
|
||||
/// For testing and development. Called in the medium loop.
|
||||
void
|
||||
AP_Mount::debug_output()
|
||||
{ Serial3.print("current - ");
|
||||
@ -478,6 +556,51 @@ AP_Mount::debug_output()
|
||||
Serial3.print(",alt ");
|
||||
Serial3.print(_target_GPS_location.alt);
|
||||
Serial3.print(" hdg to targ ");
|
||||
Serial3.print(degrees(_yaw_control_angle));
|
||||
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(RC_Channel* rc, int16_t angle, int16_t angle_min, int16_t angle_max)
|
||||
{
|
||||
if (rc) {
|
||||
// saturate to the closest angle limit if outside of [min max] angle interval
|
||||
rc->servo_out = closest_limit(angle, &angle_min, &angle_max);
|
||||
// This is done every time because the user might change the min, max values on the fly
|
||||
rc->set_range(angle_min, angle_max);
|
||||
// convert angle to PWM using a linear transformation (ignores trimming because the servo limits might not be symmetric)
|
||||
rc->calc_pwm();
|
||||
rc->output();
|
||||
}
|
||||
}
|
||||
|
||||
@ -29,12 +29,6 @@
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <../RC_Channel/RC_Channel_aux.h>
|
||||
|
||||
// #defines to control function of RC Channel used to manually provide angular offset to AP_Mount when we can't use RC_Channel_aux (which is the case for ArduCopter).
|
||||
#define AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED 0
|
||||
#define AP_MOUNT_MANUAL_RC_FUNCTION_ROLL 1
|
||||
#define AP_MOUNT_MANUAL_RC_FUNCTION_PITCH 2
|
||||
#define AP_MOUNT_MANUAL_RC_FUNCTION_YAW 3
|
||||
|
||||
class AP_Mount
|
||||
{
|
||||
public:
|
||||
@ -43,9 +37,10 @@ public:
|
||||
|
||||
//enums
|
||||
enum MountType{
|
||||
k_pan_tilt = 0, ///< yaw-pitch
|
||||
k_tilt_roll = 1, ///< pitch-roll
|
||||
k_pan_tilt_roll = 2, ///< yaw-pitch-roll
|
||||
k_unknown = 0, ///< unknown type
|
||||
k_pan_tilt = 1, ///< yaw-pitch
|
||||
k_tilt_roll = 2, ///< pitch-roll
|
||||
k_pan_tilt_roll = 3, ///< yaw-pitch-roll
|
||||
};
|
||||
|
||||
// MAVLink methods
|
||||
@ -62,11 +57,6 @@ public:
|
||||
void debug_output(); ///< For testing and development. Called in the medium loop.
|
||||
// Accessors
|
||||
enum MountType get_mount_type() { return _mount_type; }
|
||||
|
||||
// to allow manual input of an angle from the pilot when RC_Channel_aux cannot be used
|
||||
void set_manual_rc_channel(RC_Channel *rc); // define which RC_Channel is to be used for manual control
|
||||
void set_manual_rc_channel_function(int8_t fn); // set whether manual rc channel controlls roll (1), pitch (2) or yaw (3).
|
||||
|
||||
// hook for eeprom variables
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
@ -75,43 +65,56 @@ private:
|
||||
//methods
|
||||
void set_mode(enum MAV_MOUNT_MODE mode);
|
||||
|
||||
void set_retract_angles(float roll, float pitch, float yaw); ///< set mount retracted position
|
||||
void set_neutral_angles(float roll, float pitch, float yaw);
|
||||
void set_control_angles(float roll, float pitch, float yaw);
|
||||
void set_retract_angles(float roll, float tilt, float pan); ///< set mount retracted position
|
||||
void set_neutral_angles(float roll, float tilt, float pan);
|
||||
void set_control_angles(float roll, float tilt, float pan);
|
||||
void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location
|
||||
|
||||
// internal methods
|
||||
void calc_GPS_target_angle(struct Location *target);
|
||||
void stabilize();
|
||||
int16_t closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max);
|
||||
void move_servo(RC_Channel* rc, int16_t angle, int16_t angle_min, int16_t angle_max);
|
||||
int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
||||
float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
||||
|
||||
//members
|
||||
AP_AHRS *_ahrs; ///< Rotation matrix from earth to plane.
|
||||
GPS *&_gps;
|
||||
const struct Location *_current_loc;
|
||||
static const float t7 = 10000000.0;
|
||||
float _roll_control_angle; ///< radians
|
||||
float _pitch_control_angle; ///< radians
|
||||
float _yaw_control_angle; ///< radians
|
||||
|
||||
float _roll_angle; ///< degrees
|
||||
float _pitch_angle; ///< degrees
|
||||
float _yaw_angle; ///< degrees
|
||||
|
||||
AP_Int8 _stab_roll; ///< (1 = yes, 0 = no)
|
||||
AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no)
|
||||
AP_Int8 _stab_yaw; ///< (1 = yes, 0 = no)
|
||||
|
||||
AP_Int8 _mount_mode;
|
||||
struct Location _target_GPS_location;
|
||||
MountType _mount_type;
|
||||
|
||||
struct Location _target_GPS_location;
|
||||
float _roll_control_angle; ///< radians
|
||||
float _tilt_control_angle; ///< radians
|
||||
float _pan_control_angle; ///< radians
|
||||
|
||||
AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
float _roll_angle; ///< degrees
|
||||
float _tilt_angle; ///< degrees
|
||||
float _pan_angle; ///< degrees
|
||||
|
||||
// EEPROM parameters
|
||||
AP_Int8 _stab_roll; ///< (1 = yes, 0 = no)
|
||||
AP_Int8 _stab_tilt; ///< (1 = yes, 0 = no)
|
||||
AP_Int8 _stab_pan; ///< (1 = yes, 0 = no)
|
||||
|
||||
AP_Int8 _mount_mode;
|
||||
// RC_Channel for providing direct angular input from pilot
|
||||
RC_Channel* _manual_rc;
|
||||
int8_t _manual_rc_function;
|
||||
AP_Int8 _roll_rc_in;
|
||||
AP_Int8 _tilt_rc_in;
|
||||
AP_Int8 _pan_rc_in;
|
||||
|
||||
AP_Int16 _roll_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 _roll_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 _tilt_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 _tilt_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 _pan_angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 _pan_angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
||||
|
||||
//AP_Int8 _joystick_speed;
|
||||
|
||||
AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
||||
AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
||||
AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = tilt, vector.z=pan
|
||||
};
|
||||
#endif
|
||||
|
||||
@ -9,95 +9,16 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] PROGMEM = {
|
||||
// @Param: FUNCTION
|
||||
// @DisplayName: Servo out function
|
||||
// @Description: Setting this to Disabled(0) will disable this output, any other value will enable the corresponding function
|
||||
// @Values: 0:Disabled,1:Manual,2:Flap,3:Flap_auto,4:Aileron,5:flaperon,6:mount_pan,7:mount_pitch,8:mount_roll,9:mount_open,10:camera_trigger,11:release
|
||||
// @Values: 0:Disabled,1:Manual,2:Flap,3:Flap_auto,4:Aileron,5:flaperon,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("FUNCTION", 1, RC_Channel_aux, function),
|
||||
|
||||
// @Param: ANGLE_MIN
|
||||
// @DisplayName: Minimum object position
|
||||
// @Description: Minimum physical angular position of the object that this servo output controls. For example a camera pan angle, an aileron angle, etc
|
||||
// @Units: centi-Degrees
|
||||
// @Range: -18000 17999
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ANGLE_MIN", 2, RC_Channel_aux, angle_min),
|
||||
|
||||
// @Param: ANGLE_MAX
|
||||
// @DisplayName: Maximum object position
|
||||
// @Description: Maximum physical angular position of the object that this servo output controls. For example a camera pan angle, an aileron angle, etc
|
||||
// @Units: centi-Degrees
|
||||
// @Range: -18000 17999
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ANGLE_MAX", 3, RC_Channel_aux, angle_max),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
/// Global pointer array, indexed by a "RC function enum" and points to the RC channel output assigned to that function/operation
|
||||
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions];
|
||||
|
||||
/// saturate to the closest angle limit if outside of [min max] angle interval
|
||||
/// input angle is in degrees * 10
|
||||
int16_t
|
||||
RC_Channel_aux::closest_limit(int16_t angle)
|
||||
{
|
||||
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
|
||||
int16_t min = angle_min / 10;
|
||||
int16_t max = angle_max / 10;
|
||||
|
||||
// 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 (min < -1800) min += 3600;
|
||||
while (min >= 1800) min -= 3600;
|
||||
while (max < -1800) max += 3600;
|
||||
while (max >= 1800) max -= 3600;
|
||||
// This is done every time because the user might change the min, max values on the fly
|
||||
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 < min) && (angle > max)){
|
||||
// angle error if min limit is used
|
||||
int16_t err_min = min - 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 - max + (angle>max?0:3600); // add 360 degrees if on the "wrong side"
|
||||
angle = err_min<err_max?min:max;
|
||||
}
|
||||
|
||||
servo_out = angle;
|
||||
// convert angle to PWM using a linear transformation (ignores trimming because the servo limits might not be symmetric)
|
||||
calc_pwm();
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// Gets the RC and integrates and then compares with the servo out angles to limit control input to servo travel.
|
||||
/// That way the user doesn't get lost. Rotationally.
|
||||
void
|
||||
RC_Channel_aux::rc_input(float *control_angle, int16_t angle)
|
||||
{
|
||||
if((radio_in < 1480 && angle < angle_max)||(radio_in > 1520 && angle > angle_min)){
|
||||
*control_angle += ( 1500 - radio_in ) * .0001; // .0001 is the control speed scaler.
|
||||
}
|
||||
}
|
||||
|
||||
/// returns the angle (degrees*100) that the RC_Channel input is receiving
|
||||
int32_t
|
||||
RC_Channel_aux::angle_input()
|
||||
{
|
||||
return (get_reverse()?-1:1) * (radio_in - radio_min) * (int32_t)(angle_max - angle_min) / (radio_max - radio_min) + (get_reverse()?angle_max:angle_min);
|
||||
}
|
||||
|
||||
/// returns the angle (radians) that the RC_Channel input is receiving
|
||||
float
|
||||
RC_Channel_aux::angle_input_rad()
|
||||
{
|
||||
return radians(angle_input()*0.01);
|
||||
}
|
||||
|
||||
/// enable_out_ch - enable the channel through APM_RC
|
||||
void
|
||||
RC_Channel_aux::enable_out_ch(unsigned char ch_nr)
|
||||
@ -131,6 +52,7 @@ RC_Channel_aux::output_ch(unsigned char ch_nr)
|
||||
/// expects the changes to take effect instantly
|
||||
/// Supports up to seven aux servo outputs (typically CH5 ... CH11)
|
||||
/// All servos must be configured with a single call to this function
|
||||
/// (do not call this twice with different parameters, the second call will reset the effect of the first call)
|
||||
void update_aux_servo_function( RC_Channel_aux* rc_a,
|
||||
RC_Channel_aux* rc_b,
|
||||
RC_Channel_aux* rc_c,
|
||||
@ -175,12 +97,13 @@ void update_aux_servo_function( RC_Channel_aux* rc_a,
|
||||
G_RC_AUX(k_flap_auto)->set_range(0,100);
|
||||
G_RC_AUX(k_aileron)->set_angle(4500);
|
||||
G_RC_AUX(k_flaperon)->set_range(0,100);
|
||||
G_RC_AUX(k_mount_yaw)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_pitch)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10);
|
||||
/*
|
||||
G_RC_AUX(k_mount_pan)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_pan]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_pan]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_tilt)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_tilt]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_tilt]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_roll)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10);
|
||||
@ -188,6 +111,7 @@ void update_aux_servo_function( RC_Channel_aux* rc_a,
|
||||
G_RC_AUX(k_cam_trigger)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10);
|
||||
*/
|
||||
G_RC_AUX(k_egg_drop)->set_range(0,100);
|
||||
}
|
||||
|
||||
|
||||
@ -3,7 +3,6 @@
|
||||
/// @file RC_Channel_aux.h
|
||||
/// @brief RC_Channel manager for auxiliary channels (5..8), with EEPROM-backed storage of constants.
|
||||
/// @author Amilcar Lucas
|
||||
/// @author Gregory Fletcher
|
||||
|
||||
#ifndef RC_CHANNEL_AUX_H_
|
||||
#define RC_CHANNEL_AUX_H_
|
||||
@ -25,9 +24,7 @@ public:
|
||||
///
|
||||
RC_Channel_aux(uint8_t ch_out) :
|
||||
RC_Channel(ch_out),
|
||||
function (0),
|
||||
angle_min (-4500), // assume -45 degrees min deflection
|
||||
angle_max (4500) // assume 45 degrees max deflection
|
||||
function (0)
|
||||
{}
|
||||
|
||||
typedef enum
|
||||
@ -38,8 +35,8 @@ public:
|
||||
k_flap_auto = 3, ///< flap automated
|
||||
k_aileron = 4, ///< aileron
|
||||
k_flaperon = 5, ///< flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
||||
k_mount_yaw = 6, ///< mount yaw (pan)
|
||||
k_mount_pitch = 7, ///< mount pitch (tilt)
|
||||
k_mount_pan = 6, ///< mount yaw (pan)
|
||||
k_mount_tilt = 7, ///< mount pitch (tilt)
|
||||
k_mount_roll = 8, ///< mount roll
|
||||
k_mount_open = 9, ///< mount open (deploy) / close (retract)
|
||||
k_cam_trigger = 10, ///< camera trigger
|
||||
@ -48,16 +45,6 @@ public:
|
||||
} Aux_servo_function_t;
|
||||
|
||||
AP_Int8 function; ///< see Aux_servo_function_t enum
|
||||
AP_Int16 angle_min; ///< min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 angle_max; ///< max angle limit of actuated surface in 0.01 degree units
|
||||
|
||||
int16_t closest_limit(int16_t angle);
|
||||
|
||||
void rc_input(float *control_angle, int16_t angle);
|
||||
|
||||
int32_t angle_input();
|
||||
|
||||
float angle_input_rad();
|
||||
|
||||
void enable_out_ch(unsigned char ch_nr);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user