2011-09-11 14:13:01 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
# include <APM_RC.h>
# include "RC_Channel_aux.h"
2012-02-11 07:54:21 -04:00
const AP_Param : : GroupInfo RC_Channel_aux : : var_info [ ] PROGMEM = {
2012-02-12 18:55:13 -04:00
AP_NESTEDGROUPINFO ( RC_Channel , 0 ) ,
2012-06-13 15:55:19 -03:00
// @Param: FUNCTION
// @DisplayName: Function assigned to this APM servo output
// @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_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release
// @User: Standard
2012-02-12 18:55:13 -04:00
AP_GROUPINFO ( " FUNCTION " , 1 , RC_Channel_aux , function ) ,
2012-06-13 15:55:19 -03:00
// @Param: ANGLE_MIN
// @DisplayName: Minimum physical angular position of the object that this servo output controls
// @Description: Minimum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc
// @Units: Degrees
// @Range: -180 180
// @Increment: .01
// @User: Standard
2012-02-12 18:55:13 -04:00
AP_GROUPINFO ( " ANGLE_MIN " , 2 , RC_Channel_aux , angle_min ) ,
2012-06-13 15:55:19 -03:00
// @Param: ANGLE_MAX
// @DisplayName: Maximum physical angular position of the object that this servo output controls
// @Description: Maximum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc
// @Units: Degrees
// @Range: -180 180
// @Increment: .01
// @User: Standard
2012-02-12 18:55:13 -04:00
AP_GROUPINFO ( " ANGLE_MAX " , 3 , RC_Channel_aux , angle_max ) ,
2012-02-12 03:23:19 -04:00
AP_GROUPEND
2012-02-11 07:54:21 -04:00
} ;
2012-06-13 15:55:19 -03:00
// 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 ] ;
2011-09-11 18:07:30 -03:00
2011-10-31 18:55:58 -03:00
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 camera limits might not be symmetric)
calc_pwm ( ) ;
return angle ;
}
2012-06-13 15:55:19 -03:00
// 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.
}
}
// Takes the desired servo angle(deg) and converts to microSeconds for PWM
// Like this: 45 deg = 2000 us ; -45 deg/1000 us. 1000us/(90*100 deg) = 0.1111111111111
void
RC_Channel_aux : : angle_out ( int16_t angle )
{
if ( angle > = angle_max ) {
angle = angle_max ;
}
if ( angle < = angle_min ) {
angle = angle_min ;
}
// Convert the angle*100 to pwm microseconds. 45 deg = 500 us.
radio_out = ( /*_reverse * */ angle * 0.1111111 ) + 1500 ;
}
2011-09-11 14:13:01 -03:00
// map a function to a servo channel and output it
void
RC_Channel_aux : : output_ch ( unsigned char ch_nr )
{
2011-09-12 15:03:44 -03:00
// take care or two corner cases
2011-09-11 14:13:01 -03:00
switch ( function )
{
case k_none : // disabled
return ;
break ;
case k_manual : // manual
radio_out = radio_in ;
break ;
}
2011-11-12 23:18:14 -04:00
_apm_rc - > OutputCh ( ch_nr , radio_out ) ;
2011-09-11 14:13:01 -03:00
}
2011-09-11 18:07:30 -03:00
2011-09-12 15:03:44 -03:00
// Update the g_rc_function array of pointers to rc_x channels
// This is to be done before rc_init so that the channels get correctly initialized.
// It also should be called periodically because the user might change the configuration and
2011-09-11 18:07:30 -03:00
// expects the changes to take effect instantly
void update_aux_servo_function ( RC_Channel_aux * rc_5 , RC_Channel_aux * rc_6 , RC_Channel_aux * rc_7 , RC_Channel_aux * rc_8 )
{
// positions 0..3 of this array never get used, but this is a stack array, so the entire array gets freed at the end of the function
2012-02-14 02:46:59 -04:00
RC_Channel_aux : : Aux_servo_function_t aux_servo_function [ 4 ] ;
aux_servo_function [ 0 ] = ( RC_Channel_aux : : Aux_servo_function_t ) rc_5 - > function . get ( ) ;
aux_servo_function [ 1 ] = ( RC_Channel_aux : : Aux_servo_function_t ) rc_6 - > function . get ( ) ;
aux_servo_function [ 2 ] = ( RC_Channel_aux : : Aux_servo_function_t ) rc_7 - > function . get ( ) ;
aux_servo_function [ 3 ] = ( RC_Channel_aux : : Aux_servo_function_t ) rc_8 - > function . get ( ) ;
2011-09-11 18:07:30 -03:00
2012-02-14 02:46:59 -04:00
for ( uint8_t i = 0 ; i < 4 ; i + + ) {
2012-02-12 21:39:16 -04:00
if ( aux_servo_function [ i ] > = RC_Channel_aux : : k_nr_aux_servo_functions ) {
// invalid setting
aux_servo_function [ i ] = RC_Channel_aux : : k_none ;
}
}
2011-09-11 18:07:30 -03:00
// Assume that no auxiliary function is used
2012-02-12 21:39:16 -04:00
for ( uint8_t i = 0 ; i < RC_Channel_aux : : k_nr_aux_servo_functions ; i + + )
2011-09-11 18:07:30 -03:00
{
g_rc_function [ i ] = NULL ;
}
// assign the RC channel to each function
2012-02-14 02:46:59 -04:00
g_rc_function [ aux_servo_function [ 0 ] ] = rc_5 ;
g_rc_function [ aux_servo_function [ 1 ] ] = rc_6 ;
g_rc_function [ aux_servo_function [ 2 ] ] = rc_7 ;
g_rc_function [ aux_servo_function [ 3 ] ] = rc_8 ;
2011-09-13 09:01:35 -03:00
2011-09-17 15:25:31 -03:00
//set auxiliary ranges
2011-09-13 09:01:35 -03:00
G_RC_AUX ( k_flap ) - > set_range ( 0 , 100 ) ;
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 ) ;
2011-10-31 18:55:58 -03:00
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_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 ) ;
G_RC_AUX ( k_mount_open ) - > set_range ( 0 , 100 ) ;
2011-09-11 18:07:30 -03:00
}