mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
Moved the RC_Channel_aux class to its own file. The includes could be improved, has anyone got any ideas how ?
This commit is contained in:
parent
c5fd792024
commit
213969202a
@ -39,6 +39,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||||
#include <PID.h> // PID library
|
#include <PID.h> // PID library
|
||||||
#include <RC_Channel.h> // RC Channel Library
|
#include <RC_Channel.h> // RC Channel Library
|
||||||
|
#include <../libraries/RC_Channel/RC_Channel_aux.h> // Auxiliary RC Channel Library (Channels 5..8)
|
||||||
#include <AP_RangeFinder.h> // Range finder library
|
#include <AP_RangeFinder.h> // Range finder library
|
||||||
#include <ModeFilter.h>
|
#include <ModeFilter.h>
|
||||||
#include <AP_Camera.h> // Photo or video camera
|
#include <AP_Camera.h> // Photo or video camera
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
#include <AP_Camera.h>
|
#include <AP_Camera.h>
|
||||||
#include <RC_Channel.h>
|
#include <../RC_Channel/RC_Channel_aux.h>
|
||||||
|
|
||||||
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_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||||
extern long wp_distance;
|
extern long wp_distance;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
#include <AP_Mount.h>
|
#include <AP_Mount.h>
|
||||||
#include <RC_Channel.h>
|
#include <../RC_Channel/RC_Channel_aux.h>
|
||||||
|
|
||||||
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_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||||
|
|
||||||
|
@ -11,7 +11,6 @@
|
|||||||
|
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <avr/eeprom.h>
|
#include <avr/eeprom.h>
|
||||||
#include <APM_RC.h>
|
|
||||||
#include "WProgram.h"
|
#include "WProgram.h"
|
||||||
#include "RC_Channel.h"
|
#include "RC_Channel.h"
|
||||||
|
|
||||||
@ -219,68 +218,3 @@ RC_Channel::norm_output()
|
|||||||
else
|
else
|
||||||
return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim);
|
return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim);
|
||||||
}
|
}
|
||||||
|
|
||||||
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;
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
switch(function)
|
|
||||||
{
|
|
||||||
case k_none: // disabled
|
|
||||||
return;
|
|
||||||
break;
|
|
||||||
case k_manual: // manual
|
|
||||||
radio_out = radio_in;
|
|
||||||
break;
|
|
||||||
case k_flap: // flaps
|
|
||||||
case k_flap_auto: // flaps automated
|
|
||||||
case k_aileron: // aileron
|
|
||||||
case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
|
||||||
case k_mount_yaw: // mount yaw (pan)
|
|
||||||
case k_mount_pitch: // mount pitch (tilt)
|
|
||||||
case k_mount_roll: // mount roll
|
|
||||||
case k_cam_trigger: // camera trigger
|
|
||||||
case k_cam_open: // camera open
|
|
||||||
case k_egg_drop: // egg drop
|
|
||||||
case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
APM_RC.OutputCh(ch_nr, radio_out);
|
|
||||||
}
|
|
||||||
|
@ -7,8 +7,6 @@
|
|||||||
#define RC_Channel_h
|
#define RC_Channel_h
|
||||||
|
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
// TODO is this include really necessary ?
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
/// @class RC_Channel
|
/// @class RC_Channel
|
||||||
/// @brief Object managing one RC channel
|
/// @brief Object managing one RC channel
|
||||||
@ -104,57 +102,4 @@ class RC_Channel{
|
|||||||
int16_t _low;
|
int16_t _low;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// @class RC_Channel_aux
|
|
||||||
/// @brief Object managing one aux. RC channel (CH5-8), with information about its function
|
|
||||||
/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements
|
|
||||||
class RC_Channel_aux : public RC_Channel{
|
|
||||||
public:
|
|
||||||
/// Constructor
|
|
||||||
///
|
|
||||||
/// @param key EEPROM storage key for the channel trim parameters.
|
|
||||||
/// @param name Optional name for the group.
|
|
||||||
///
|
|
||||||
RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) :
|
|
||||||
RC_Channel(key, name),
|
|
||||||
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name
|
|
||||||
angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection
|
|
||||||
angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection
|
|
||||||
{}
|
|
||||||
|
|
||||||
typedef enum
|
|
||||||
{
|
|
||||||
k_none = 0, // disabled
|
|
||||||
k_manual = 1, // manual, just pass-thru the RC in signal
|
|
||||||
k_flap = 2, // flap
|
|
||||||
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_roll = 8, // mount roll
|
|
||||||
k_cam_trigger = 9, // camera trigger
|
|
||||||
k_cam_open = 10, // camera open
|
|
||||||
k_egg_drop = 11, // egg drop
|
|
||||||
k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one)
|
|
||||||
} Aux_servo_function_t;
|
|
||||||
|
|
||||||
// TODO It would be great if the "packed" attribute could be added to this somehow
|
|
||||||
// It would probably save some memory. But it can only be added to enums and not to typedefs :(
|
|
||||||
/*AP_VARDEF(Aux_servo_function_t, Aux_srv_func); // defines AP_Aux_srv_func
|
|
||||||
|
|
||||||
AP_Aux_srv_func function;*/ // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop
|
|
||||||
AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop
|
|
||||||
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); // saturate to the closest angle limit if outside of min max angle interval
|
|
||||||
|
|
||||||
void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
69
libraries/RC_Channel/RC_Channel_aux.cpp
Normal file
69
libraries/RC_Channel/RC_Channel_aux.cpp
Normal file
@ -0,0 +1,69 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
|
#include <APM_RC.h>
|
||||||
|
#include "RC_Channel_aux.h"
|
||||||
|
|
||||||
|
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;
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
switch(function)
|
||||||
|
{
|
||||||
|
case k_none: // disabled
|
||||||
|
return;
|
||||||
|
break;
|
||||||
|
case k_manual: // manual
|
||||||
|
radio_out = radio_in;
|
||||||
|
break;
|
||||||
|
case k_flap: // flaps
|
||||||
|
case k_flap_auto: // flaps automated
|
||||||
|
case k_aileron: // aileron
|
||||||
|
case k_flaperon: // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
||||||
|
case k_mount_yaw: // mount yaw (pan)
|
||||||
|
case k_mount_pitch: // mount pitch (tilt)
|
||||||
|
case k_mount_roll: // mount roll
|
||||||
|
case k_cam_trigger: // camera trigger
|
||||||
|
case k_cam_open: // camera open
|
||||||
|
case k_egg_drop: // egg drop
|
||||||
|
case k_nr_aux_servo_functions: // dummy, just to avoid a compiler warning
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
APM_RC.OutputCh(ch_nr, radio_out);
|
||||||
|
}
|
56
libraries/RC_Channel/RC_Channel_aux.h
Normal file
56
libraries/RC_Channel/RC_Channel_aux.h
Normal file
@ -0,0 +1,56 @@
|
|||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
|
||||||
|
/// @file RC_Channel_aux.h
|
||||||
|
/// @brief RC_Channel manager, with EEPROM-backed storage of constants.
|
||||||
|
/// @author Amilcar Lucas
|
||||||
|
|
||||||
|
#ifndef RC_CHANNEL_AUX_H_
|
||||||
|
#define RC_CHANNEL_AUX_H_
|
||||||
|
|
||||||
|
#include "RC_Channel.h"
|
||||||
|
|
||||||
|
/// @class RC_Channel_aux
|
||||||
|
/// @brief Object managing one aux. RC channel (CH5-8), with information about its function
|
||||||
|
/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements
|
||||||
|
class RC_Channel_aux : public RC_Channel{
|
||||||
|
public:
|
||||||
|
/// Constructor
|
||||||
|
///
|
||||||
|
/// @param key EEPROM storage key for the channel trim parameters.
|
||||||
|
/// @param name Optional name for the group.
|
||||||
|
///
|
||||||
|
RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) :
|
||||||
|
RC_Channel(key, name),
|
||||||
|
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name
|
||||||
|
angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection
|
||||||
|
angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection
|
||||||
|
{}
|
||||||
|
|
||||||
|
typedef enum
|
||||||
|
{
|
||||||
|
k_none = 0, // disabled
|
||||||
|
k_manual = 1, // manual, just pass-thru the RC in signal
|
||||||
|
k_flap = 2, // flap
|
||||||
|
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_roll = 8, // mount roll
|
||||||
|
k_cam_trigger = 9, // camera trigger
|
||||||
|
k_cam_open = 10, // camera open
|
||||||
|
k_egg_drop = 11, // egg drop
|
||||||
|
k_nr_aux_servo_functions // This must be the last enum value (only add new values _before_ this one)
|
||||||
|
} Aux_servo_function_t;
|
||||||
|
|
||||||
|
AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop
|
||||||
|
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); // saturate to the closest angle limit if outside of min max angle interval
|
||||||
|
|
||||||
|
void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* RC_CHANNEL_AUX_H_ */
|
Loading…
Reference in New Issue
Block a user