mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
ab730ff919
It is fully configurable with the mission planner, there is no need to change the source code to adapt to your setup. It needs more testing, but the SIL is not working for me.
103 lines
4.0 KiB
C++
103 lines
4.0 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
#include <APM_RC.h>
|
|
#include "RC_Channel_aux.h"
|
|
|
|
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
|
|
|
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;
|
|
}
|
|
|
|
// map a function to a servo channel and output it
|
|
void
|
|
RC_Channel_aux::output_ch(unsigned char ch_nr)
|
|
{
|
|
// take care or two corner cases
|
|
switch(function)
|
|
{
|
|
case k_none: // disabled
|
|
return;
|
|
break;
|
|
case k_manual: // manual
|
|
radio_out = radio_in;
|
|
break;
|
|
}
|
|
|
|
APM_RC.OutputCh(ch_nr, radio_out);
|
|
}
|
|
|
|
// 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
|
|
// 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
|
|
RC_Channel_aux::Aux_servo_function_t aux_servo_function[NUM_CHANNELS]; // the function of the aux. servos
|
|
aux_servo_function[CH_5] = (RC_Channel_aux::Aux_servo_function_t)rc_5->function.get();
|
|
aux_servo_function[CH_6] = (RC_Channel_aux::Aux_servo_function_t)rc_6->function.get();
|
|
aux_servo_function[CH_7] = (RC_Channel_aux::Aux_servo_function_t)rc_7->function.get();
|
|
aux_servo_function[CH_8] = (RC_Channel_aux::Aux_servo_function_t)rc_8->function.get();
|
|
|
|
// Assume that no auxiliary function is used
|
|
for (int i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++)
|
|
{
|
|
g_rc_function[i] = NULL;
|
|
}
|
|
|
|
// assign the RC channel to each function
|
|
g_rc_function[aux_servo_function[CH_5]] = rc_5;
|
|
g_rc_function[aux_servo_function[CH_6]] = rc_6;
|
|
g_rc_function[aux_servo_function[CH_7]] = rc_7;
|
|
g_rc_function[aux_servo_function[CH_8]] = rc_8;
|
|
|
|
//set auxiliary ranges
|
|
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);
|
|
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);
|
|
}
|