aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RC_Channel: split SRV_Channel into separate directory

Browse Source
This commit is contained in:
Andrew Tridgell 2017-01-07 12:02:32 +11:00
parent 64aa13e7f4
commit 0791186cd2
4 changed files with 3 additions and 1281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/RC_Channel/RC_Channel.cpp
View File

@ -282,10 +282,10 @@ void RC_Channel::save_eeprom(void)
the current radio_in value using the specified dead_zone
*/
int16_t
RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t trim)
RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim)
{
int16_t radio_trim_high = trim + _dead_zone;
int16_t radio_trim_low = trim - _dead_zone;
int16_t radio_trim_high = _trim + _dead_zone;
int16_t radio_trim_low = _trim - _dead_zone;
// prevent div by 0
if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)

321
libraries/RC_Channel/SRV_Channel.cpp
View File

@ -1,321 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
SRV_Channel.cpp - object to separate input and output channel
ranges, trim and reversal
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include "SRV_Channel.h"
#include "RC_Channel.h"
extern const AP_HAL::HAL& hal;
SRV_Channel *SRV_Channels::channels;
bool SRV_Channels::disabled_passthrough;
bool SRV_Channels::initialised;
Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions};
SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];
SRV_Channel::servo_mask_t SRV_Channel::have_pwm_mask;
const AP_Param::GroupInfo SRV_Channel::var_info[] = {
// @Param: MIN
// @DisplayName: Minimum PWM
// @Description: minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MIN", 1, SRV_Channel, servo_min, 1100),
// @Param: MAX
// @DisplayName: Maximum PWM
// @Description: maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Standard
AP_GROUPINFO("MAX", 2, SRV_Channel, servo_max, 1900),
// @Param: TRIM
// @DisplayName: Trim PWM
// @Description: Trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
// @Units: pwm
// @Range: 800 2200
// @Increment: 1
// @User: Standard
AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500),
// @Param: REVERSED
// @DisplayName: Servo reverse
// @Description: Reverse servo operation. Set to 0 for normal operation. Set to 1 to reverse this channel.
// @Values: 0:Normal,1:Reversed
// @User: Standard
AP_GROUPINFO("REVERSED", 4, SRV_Channel, reversed, 0),
// @Param: FUNCTION
// @DisplayName: Servo output function
// @Description: Function assigned to this servo. Seeing this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function
// @Values: 0:Disabled,1:RCPassThru,2:Flap,3:Flap_auto,4:Aileron,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release,12:mount2_pan,13:mount2_tilt,14:mount2_roll,15:mount2_open,16:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,68:Choke,69:Starter,70:Throttle
// @User: Standard
AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0),
AP_GROUPEND
};
const AP_Param::GroupInfo SRV_Channels::var_info[] = {
// @Group: 1_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[0], "1_", 1, SRV_Channels, SRV_Channel),
// @Group: 2_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[1], "2_", 2, SRV_Channels, SRV_Channel),
// @Group: 3_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[2], "3_", 3, SRV_Channels, SRV_Channel),
// @Group: 4_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[3], "4_", 4, SRV_Channels, SRV_Channel),
// @Group: 5_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[4], "5_", 5, SRV_Channels, SRV_Channel),
// @Group: 6_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[5], "6_", 6, SRV_Channels, SRV_Channel),
// @Group: 7_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[6], "7_", 7, SRV_Channels, SRV_Channel),
// @Group: 8_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[7], "8_", 8, SRV_Channels, SRV_Channel),
// @Group: 9_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[8], "9_", 9, SRV_Channels, SRV_Channel),
// @Group: 10_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[9], "10_", 10, SRV_Channels, SRV_Channel),
// @Group: 11_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[10], "11_", 11, SRV_Channels, SRV_Channel),
// @Group: 12_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[11], "12_", 12, SRV_Channels, SRV_Channel),
// @Group: 13_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[12], "13_", 13, SRV_Channels, SRV_Channel),
// @Group: 14_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[13], "14_", 14, SRV_Channels, SRV_Channel),
// @Group: 15_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[14], "15_", 15, SRV_Channels, SRV_Channel),
// @Group: 16_
// @Path: SRV_Channel.cpp
AP_SUBGROUPINFO(obj_channels[15], "16_", 16, SRV_Channels, SRV_Channel),
// @Param: _AUTO_TRIM
// @DisplayName: Automatic servo trim
// @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights.
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO("_AUTO_TRIM", 17, SRV_Channels, auto_trim, 0),
AP_GROUPEND
};
SRV_Channel::SRV_Channel(void)
{
AP_Param::setup_object_defaults(this, var_info);
// start with all pwm at zero
have_pwm_mask = ~uint16_t(0);
}
/*
constructor
*/
SRV_Channels::SRV_Channels(const RCMapper &_rcmap) :
rcmap(_rcmap)
{
channels = obj_channels;
// set defaults from the parameter table
AP_Param::setup_object_defaults(this, var_info);
// setup ch_num on channels
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].ch_num = i;
}
}
/*
save adjusted trims
*/
void SRV_Channels::save_trim(void)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (trimmed_mask & (1U<<i)) {
channels[i].servo_trim.set_and_save(channels[i].servo_trim.get());
}
}
trimmed_mask = 0;
}
// convert a 0..range_max to a pwm
uint16_t SRV_Channel::pwm_from_range(int16_t scaled_value) const
{
if (servo_max <= servo_min || high_out == 0) {
return servo_min;
}
if (scaled_value >= high_out) {
scaled_value = high_out;
}
if (scaled_value < 0) {
scaled_value = 0;
}
if (reversed) {
scaled_value = high_out - scaled_value;
}
return servo_min + ((int32_t)scaled_value * (int32_t)(servo_max - servo_min)) / (int32_t)high_out;
}
// convert a -angle_max..angle_max to a pwm
uint16_t SRV_Channel::pwm_from_angle(int16_t scaled_value) const
{
if (reversed) {
scaled_value = -scaled_value;
}
if (scaled_value > 0) {
return servo_trim + ((int32_t)scaled_value * (int32_t)(servo_max - servo_trim)) / (int32_t)high_out;
} else {
return servo_trim - (-(int32_t)scaled_value * (int32_t)(servo_trim - servo_min)) / (int32_t)high_out;
}
}
void SRV_Channel::calc_pwm(int16_t output_scaled)
{
if (have_pwm_mask & (1U<<ch_num)) {
return;
}
uint16_t pwm;
if (type_angle) {
pwm = pwm_from_angle(output_scaled);
} else {
pwm = pwm_from_range(output_scaled);
}
set_output_pwm(pwm);
}
void SRV_Channels::output_trim_all(void)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].set_output_pwm(channels[i].servo_trim);
}
}
void SRV_Channels::setup_failsafe_trim_all(void)
{
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
hal.rcout->set_failsafe_pwm(1U<<channels[i].ch_num, channels[i].servo_trim);
}
}
void SRV_Channel::set_output_pwm(uint16_t pwm)
{
output_pwm = pwm;
have_pwm_mask |= (1U<<ch_num);
}
// set angular range of scaled output
void SRV_Channel::set_angle(int16_t angle)
{
type_angle = true;
high_out = angle;
type_setup = true;
}
// set range of scaled output
void SRV_Channel::set_range(uint16_t high)
{
type_angle = false;
high_out = high;
type_setup = true;
}
/*
get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
*/
float SRV_Channel::get_output_norm(void)
{
uint16_t mid = (servo_max + servo_min) / 2;
float ret;
if (mid <= servo_min) {
return 0;
}
if (output_pwm < mid) {
ret = (float)(output_pwm - mid) / (float)(mid - servo_min);
} else if (output_pwm > mid) {
ret = (float)(output_pwm - mid) / (float)(servo_max - mid);
} else {
ret = 0;
}
if (get_reversed()) {
ret = -ret;
}
return ret;
}
uint16_t SRV_Channel::get_limit_pwm(LimitValue limit) const
{
switch (limit) {
case SRV_CHANNEL_LIMIT_TRIM:
return servo_trim;
case SRV_CHANNEL_LIMIT_MIN:
return servo_min;
case SRV_CHANNEL_LIMIT_MAX:
return servo_max;
case SRV_CHANNEL_LIMIT_ZERO_PWM:
default:
return 0;
}
}
/*
run calc_pwm for all channels
*/
void SRV_Channels::calc_pwm(void)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].calc_pwm(functions[channels[i].function].output_scaled);
}
}

388
libraries/RC_Channel/SRV_Channel.h
View File

@ -1,388 +0,0 @@
/*
control of servo output ranges, trim and servo reversal. This can
optionally be used to provide separation of input and output channel
ranges so that RCn_MIN, RCn_MAX, RCn_TRIM and RCn_REV only apply to
the input side of RC_Channel
It works by running servo output calculations as normal, then
re-mapping the output according to the servo MIN/MAX/TRIM/REV from
this object
Only 4 channels of ranges are defined as those match the input
channels for R/C sticks
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_Common/Bitmask.h>
#include "RC_Channel.h"
#define NUM_SERVO_CHANNELS 16
class SRV_Channels;
/*
class SRV_Channel. The class SRV_Channels contains an array of
SRV_Channel objects. This is done to fit within the AP_Param limit
of 64 parameters per object.
*/
class SRV_Channel {
public:
friend class SRV_Channels;
// constructor
SRV_Channel(void);
static const struct AP_Param::GroupInfo var_info[];
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_unused1 = 5, ///< unused function
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
k_egg_drop = 11, ///< egg drop
k_mount2_pan = 12, ///< mount2 yaw (pan)
k_mount2_tilt = 13, ///< mount2 pitch (tilt)
k_mount2_roll = 14, ///< mount2 roll
k_mount2_open = 15, ///< mount2 open (deploy) / close (retract)
k_dspoiler1 = 16, ///< differential spoiler 1 (left wing)
k_dspoiler2 = 17, ///< differential spoiler 2 (right wing)
k_aileron_with_input = 18, ///< aileron, with rc input
k_elevator = 19, ///< elevator
k_elevator_with_input = 20, ///< elevator, with rc input
k_rudder = 21, ///< secondary rudder channel
k_sprayer_pump = 22, ///< crop sprayer pump channel
k_sprayer_spinner = 23, ///< crop sprayer spinner channel
k_flaperon1 = 24, ///< flaperon, left wing
k_flaperon2 = 25, ///< flaperon, right wing
k_steering = 26, ///< ground steering, used to separate from rudder
k_parachute_release = 27, ///< parachute release
k_gripper = 28, ///< gripper
k_landing_gear_control = 29, ///< landing gear controller
k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters
k_heli_rsc = 31, ///< helicopter RSC output
k_heli_tail_rsc = 32, ///< helicopter tail RSC output
k_motor1 = 33, ///< these allow remapping of copter motors
k_motor2 = 34,
k_motor3 = 35,
k_motor4 = 36,
k_motor5 = 37,
k_motor6 = 38,
k_motor7 = 39,
k_motor8 = 40,
k_motor_tilt = 41, ///< tiltrotor motor tilt control
k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs
k_rcin2 = 52,
k_rcin3 = 53,
k_rcin4 = 54,
k_rcin5 = 55,
k_rcin6 = 56,
k_rcin7 = 57,
k_rcin8 = 58,
k_rcin9 = 59,
k_rcin10 = 60,
k_rcin11 = 61,
k_rcin12 = 62,
k_rcin13 = 63,
k_rcin14 = 64,
k_rcin15 = 65,
k_rcin16 = 66,
k_ignition = 67,
k_choke = 68,
k_starter = 69,
k_throttle = 70,
k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one)
} Aux_servo_function_t;
// used to get min/max/trim limit value based on reverse
enum LimitValue {
SRV_CHANNEL_LIMIT_TRIM,
SRV_CHANNEL_LIMIT_MIN,
SRV_CHANNEL_LIMIT_MAX,
SRV_CHANNEL_LIMIT_ZERO_PWM
};
// a special scaled output value that is recognised as meaning no pwm output
static const int16_t ZERO_PWM = INT16_MIN;
// set the output value as a pwm value
void set_output_pwm(uint16_t pwm);
// get the output value as a pwm value
uint16_t get_output_pwm(void) const { return output_pwm; }
// set angular range of scaled output
void set_angle(int16_t angle);
// set range of scaled output. Low is always zero
void set_range(uint16_t high);
// return true if the channel is reversed
bool get_reversed(void) const {
return reversed?true:false;
}
// set MIN/MAX parameters
void set_output_min(uint16_t pwm) {
servo_min.set(pwm);
}
void set_output_max(uint16_t pwm) {
servo_max.set(pwm);
}
// get MIN/MAX/TRIM parameters
uint16_t get_output_min(void) const {
return servo_min;
}
uint16_t get_output_max(void) const {
return servo_max;
}
uint16_t get_trim(void) const {
return servo_trim;
}
private:
AP_Int16 servo_min;
AP_Int16 servo_max;
AP_Int16 servo_trim;
// reversal, following convention that 1 means reversed, 0 means normal
AP_Int8 reversed;
AP_Int8 function;
// a pending output value as PWM
uint16_t output_pwm;
// true for angle output type
bool type_angle:1;
// set_range() or set_angle() has been called
bool type_setup:1;
// the hal channel number
uint8_t ch_num;
// high point of angle or range output
uint16_t high_out;
// convert a 0..range_max to a pwm
uint16_t pwm_from_range(int16_t scaled_value) const;
// convert a -angle_max..angle_max to a pwm
uint16_t pwm_from_angle(int16_t scaled_value) const;
// convert a scaled output to a pwm value
void calc_pwm(int16_t output_scaled);
// output value based on function
void output_ch(void);
// setup output type and range based on function
void aux_servo_function_setup(void);
// return PWM for a given limit value
uint16_t get_limit_pwm(LimitValue limit) const;
// get normalised output from -1 to 1
float get_output_norm(void);
// a bitmask type wide enough for NUM_SERVO_CHANNELS
typedef uint16_t servo_mask_t;
// mask of channels where we have a output_pwm value. Cleared when a
// scaled value is written.
static servo_mask_t have_pwm_mask;
};
/*
class SRV_Channels
*/
class SRV_Channels {
public:
// constructor
SRV_Channels(const RCMapper &_rcmap);
static const struct AP_Param::GroupInfo var_info[];
// set the default function for a channel
static void set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function);
// set output value for a function channel as a pwm value
static void set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value);
// set output value for a function channel as a pwm value on the first matching channel
static void set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t value);
// set output value for a function channel as a scaled value. This
// calls calc_pwm() to also set the pwm value
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value);
// get scaled output for the given function type.
static int16_t get_output_scaled(SRV_Channel::Aux_servo_function_t function);
// get pwm output for the first channel of the given function type.
static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value);
// get normalised output (-1 to 1 for angle, 0 to 1 for range). Value is taken from pwm value
// return zero on error.
static float get_output_norm(SRV_Channel::Aux_servo_function_t function);
// limit slew rate to given limit in percent per second
static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate);
// call output_ch() on all channels
static void output_ch_all(void);
// take current radio_out for first 4 channels and remap using
// servo ranges if enabled
void remap_servo_output(void);
// setup output ESC scaling based on a channels MIN/MAX
void set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function);
// return true when auto_trim enabled
bool auto_trim_enabled(void) const { return auto_trim; }
// adjust trim of a channel by a small increment
void adjust_trim(SRV_Channel::Aux_servo_function_t function, float v);
// save trims
void save_trim(void);
// setup for a reversible k_throttle (from -100 to 100)
void set_reversible_throttle(void) {
flags.k_throttle_reversible = true;
}
// set all outputs to the TRIM value
static void output_trim_all(void);
// setup IO failsafe for all channels to trim
static void setup_failsafe_trim_all(void);
// set output for all channels matching the given function type, allow radio_trim to center servo
static void set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value);
// set and save the trim for a function channel to radio_in on matching input channel
static void set_trim_to_radio_in_for(SRV_Channel::Aux_servo_function_t function);
// set the trim for a function channel to min of the channel
static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function);
// set the trim for a function channel to given pwm
static void set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm);
// set output to min value
static void set_output_to_min(SRV_Channel::Aux_servo_function_t function);
// set output to max value
static void set_output_to_max(SRV_Channel::Aux_servo_function_t function);
// set output to trim value
static void set_output_to_trim(SRV_Channel::Aux_servo_function_t function);
// copy radio_in to radio_out
static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false);
// setup failsafe for an auxiliary channel function, by pwm
static void set_failsafe_pwm(SRV_Channel::SRV_Channel::Aux_servo_function_t function, uint16_t pwm);
// setup failsafe for an auxiliary channel function
static void set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit);
// setup safety for an auxiliary channel function (used when disarmed)
static void set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit);
// set servo to a LimitValue
static void set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit);
// return true if a function is assigned to a channel
static bool function_assigned(SRV_Channel::Aux_servo_function_t function);
// set a servo_out value, and angle range, then calc_pwm
static void move_servo(SRV_Channel::Aux_servo_function_t function,
int16_t value, int16_t angle_min, int16_t angle_max);
// assign and enable auxiliary channels
static void enable_aux_servos(void);
// prevent a channel from being used for auxiliary functions
static void disable_aux_channel(uint8_t channel);
// return the current function for a channel
static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel);
// refresh aux servo to function mapping
static void update_aux_servo_function(void);
// set default channel for an auxiliary function
static bool set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel);
// find first channel that a function is assigned to
static bool find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan);
// find first channel that a function is assigned to, returning SRV_Channel object
static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan=-1);
// call set_angle() on matching channels
static void set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle);
// call set_range() on matching channels
static void set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range);
// control pass-thru of channels
void disable_passthrough(bool disable) {
disabled_passthrough = disable;
}
static bool passthrough_disabled(void) {
return disabled_passthrough;
}
// calculate PWM for all channels
static void calc_pwm(void);
static SRV_Channel *srv_channel(uint8_t i) {
return i<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
}
private:
const RCMapper &rcmap;
struct {
bool k_throttle_reversible:1;
} flags;
static bool disabled_passthrough;
uint16_t trimmed_mask;
static Bitmask function_mask;
static bool initialised;
// this static arrangement is to avoid having static objects in AP_Param tables
static SRV_Channel *channels;
SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
static struct srv_function {
// mask of what channels this applies to
SRV_Channel::servo_mask_t channel_mask;
// scaled output for this function
int16_t output_scaled;
} functions[SRV_Channel::k_nr_aux_servo_functions];
AP_Int8 auto_trim;
// initialise parameters from RC_Channel
void initialise_parameters(void);
};

569
libraries/RC_Channel/SRV_Channel_aux.cpp
View File

@ -1,569 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
SRV_Channel_aux.cpp - handling of servo auxillary functions
*/
#include "SRV_Channel.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
/// map a function to a servo channel and output it
void SRV_Channel::output_ch(void)
{
int8_t passthrough_from = -1;
// take care of special function cases
switch(function)
{
case k_manual: // manual
passthrough_from = ch_num;
break;
case k_rcin1 ... k_rcin16: // rc pass-thru
passthrough_from = int8_t(function - k_rcin1);
break;
case k_motor1 ... k_motor8:
// handled by AP_Motors::rc_write()
return;
}
if (passthrough_from != -1) {
// we are doing passthrough from input to output for this channel
RC_Channel *rc = RC_Channels::rc_channel(passthrough_from);
if (rc) {
if (SRV_Channels::passthrough_disabled()) {
output_pwm = rc->get_radio_trim();
} else {
output_pwm = rc->get_radio_in();
}
}
}
hal.rcout->write(ch_num, output_pwm);
}
/*
call output_ch() on all channels
*/
void SRV_Channels::output_ch_all(void)
{
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
channels[i].output_ch();
}
}
/*
return the current function for a channel
*/
SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel)
{
if (channel < NUM_SERVO_CHANNELS) {
return (SRV_Channel::Aux_servo_function_t)channels[channel].function.get();
}
return SRV_Channel::k_none;
}
/*
setup a channels aux servo function
*/
void SRV_Channel::aux_servo_function_setup(void)
{
if (type_setup) {
return;
}
switch (function) {
case k_flap:
case k_flap_auto:
case k_egg_drop:
set_range(100);
break;
case k_heli_rsc:
case k_heli_tail_rsc:
case k_motor_tilt:
set_range(1000);
break;
case k_aileron_with_input:
case k_elevator_with_input:
case k_aileron:
case k_elevator:
case k_dspoiler1:
case k_dspoiler2:
case k_rudder:
case k_steering:
case k_flaperon1:
case k_flaperon2:
set_angle(4500);
break;
case k_throttle:
// fixed wing throttle
set_range(100);
break;
default:
break;
}
}
/// setup the output range types of all functions
void SRV_Channels::update_aux_servo_function(void)
{
function_mask.clearall();
for (uint8_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
functions[i].channel_mask = 0;
}
// set auxiliary ranges
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
channels[i].aux_servo_function_setup();
function_mask.set((uint8_t)channels[i].function.get());
functions[channels[i].function.get()].channel_mask |= 1U<<i;
}
initialised = true;
}
/// Should be called after the the servo functions have been initialized
void SRV_Channels::enable_aux_servos()
{
update_aux_servo_function();
// enable all channels that are set to a valid function. This
// includes k_none servos, which allows those to get their initial
// trim value on startup
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)channels[i].function.get();
// see if it is a valid function
if (function < SRV_Channel::k_nr_aux_servo_functions) {
hal.rcout->enable_ch(channels[i].ch_num);
}
}
}
/*
set radio_out for all channels matching the given function type
*/
void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) {
channels[i].set_output_pwm(value);
channels[i].output_ch();
}
}
}
/*
set radio_out for all channels matching the given function type
trim the output assuming a 1500 center on the given value
*/
void
SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) {
int16_t value2 = value - 1500 + channels[i].get_trim();
channels[i].set_output_pwm(constrain_int16(value2,channels[i].get_output_min(),channels[i].get_output_max()));
channels[i].output_ch();
}
}
}
/*
set and save the trim value to radio_in for all channels matching
the given function type
*/
void
SRV_Channels::set_trim_to_radio_in_for(SRV_Channel::Aux_servo_function_t function)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) {
RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num);
if (rc && rc->get_radio_in() != 0) {
rc->set_radio_trim(rc->get_radio_in());
rc->save_radio_trim();
}
}
}
}
/*
copy radio_in to radio_out for a given function
*/
void
SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) {
RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num);
if (rc == nullptr) {
continue;
}
if (do_input_output) {
rc->read();
}
channels[i].set_output_pwm(rc->get_radio_in());
if (do_input_output) {
channels[i].output_ch();
}
}
}
}
/*
setup failsafe value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
hal.rcout->set_failsafe_pwm(1U<<ch.ch_num, pwm);
}
}
}
/*
setup failsafe value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
uint16_t pwm = ch.get_limit_pwm(limit);
hal.rcout->set_failsafe_pwm(1U<<ch.ch_num, pwm);
}
}
}
/*
setup safety value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
const SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
uint16_t pwm = ch.get_limit_pwm(limit);
hal.rcout->set_safety_pwm(1U<<ch.ch_num, pwm);
}
}
}
/*
set radio output value for an auxiliary function type to a LimitValue
*/
void
SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
uint16_t pwm = ch.get_limit_pwm(limit);
ch.set_output_pwm(pwm);
if (ch.function.get() == SRV_Channel::k_manual) {
RC_Channel *rc = RC_Channels::rc_channel(ch.ch_num);
if (rc != nullptr) {
// in order for output_ch() to work for k_manual we
// also have to override radio_in
rc->set_radio_in(pwm);
}
}
}
}
}
/*
return true if a particular function is assigned to at least one RC channel
*/
bool
SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function)
{
return function_mask.get(uint16_t(function));
}
/*
set servo_out and angle_min/max, then calc_pwm and output a
value. This is used to move a AP_Mount servo
*/
void
SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function,
int16_t value, int16_t angle_min, int16_t angle_max)
{
if (!function_assigned(function)) {
return;
}
if (angle_max <= angle_min) {
return;
}
float v = float(value - angle_min) / float(angle_max - angle_min);
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
SRV_Channel &ch = channels[i];
if (ch.function.get() == function) {
uint16_t pwm = ch.servo_min + v * (ch.servo_max - ch.servo_min);
ch.set_output_pwm(pwm);
}
}
}
/*
set the default channel an auxiliary output function should be on
*/
bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel)
{
if (!initialised) {
update_aux_servo_function();
}
if (function_assigned(function)) {
// already assigned
return true;
}
if (channels[channel].function != SRV_Channel::k_none) {
if (channels[channel].function == function) {
return true;
}
hal.console->printf("Channel %u already assigned %u\n",
(unsigned)channel,
(unsigned)channels[channel].function);
return false;
}
channels[channel].type_setup = false;
channels[channel].function.set(function);
channels[channel].aux_servo_function_setup();
function_mask.set((uint8_t)function);
return true;
}
// find first channel that a function is assigned to
bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan)
{
if (!initialised) {
update_aux_servo_function();
}
if (!function_assigned(function)) {
return false;
}
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
chan = channels[i].ch_num;
return true;
}
}
return false;
}
/*
get a pointer to first auxillary channel for a channel function
*/
SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan)
{
uint8_t chan;
if (default_chan >= 0) {
set_aux_channel_default(function, default_chan);
}
if (!find_channel(function, chan)) {
return nullptr;
}
return &channels[chan];
}
void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value)
{
if (function < SRV_Channel::k_nr_aux_servo_functions) {
functions[function].output_scaled = value;
SRV_Channel::have_pwm_mask &= ~functions[function].channel_mask;
}
}
int16_t SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function)
{
if (function < SRV_Channel::k_nr_aux_servo_functions) {
return functions[function].output_scaled;
}
return 0;
}
// set the trim for a function channel to given pwm
void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].servo_trim.set(pwm);
}
}
}
// set the trim for a function channel to min output
void SRV_Channels::set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].servo_trim.set(channels[i].get_reversed()?channels[i].servo_max:channels[i].servo_min);
}
}
}
/*
set the default function for a channel
*/
void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function)
{
if (chan < NUM_SERVO_CHANNELS) {
channels[chan].function.set_default((uint8_t)function);
}
}
void SRV_Channels::set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function)
{
uint8_t chan;
if (find_channel(function, chan)) {
hal.rcout->set_esc_scaling(channels[chan].get_output_min(), channels[chan].get_output_max());
}
}
/*
auto-adjust channel trim from an integrator value. Positive v means
adjust trim up. Negative means decrease
*/
void SRV_Channels::adjust_trim(SRV_Channel::Aux_servo_function_t function, float v)
{
if (is_zero(v)) {
return;
}
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
SRV_Channel &c = channels[i];
if (function != (SRV_Channel::Aux_servo_function_t)(c.function.get())) {
continue;
}
float change = c.reversed?-v:v;
uint16_t new_trim = c.servo_trim;
float trim_scaled = float(c.servo_trim - c.servo_min) / (c.servo_max - c.servo_min);
if (change > 0 && trim_scaled < 0.6f) {
new_trim++;
} else if (change < 0 && trim_scaled > 0.4f) {
new_trim--;
} else {
return;
}
c.servo_trim.set(new_trim);
trimmed_mask |= 1U<<i;
}
}
// get pwm output for the first channel of the given function type.
bool SRV_Channels::get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value)
{
uint8_t chan;
if (!find_channel(function, chan)) {
return false;
}
channels[chan].calc_pwm(functions[function].output_scaled);
value = channels[chan].output_pwm;
return true;
}
// set output pwm to trim for the given function
void SRV_Channels::set_output_to_trim(SRV_Channel::Aux_servo_function_t function)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_output_pwm(channels[i].servo_trim);
}
}
}
// set output pwm to for first matching channel
void SRV_Channels::set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t pwm)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_output_pwm(pwm);
break;
}
}
}
/*
get the normalised output for a channel function from the pwm value
of the first matching channel
*/
float SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t function)
{
uint8_t chan;
if (!find_channel(function, chan)) {
return 0;
}
channels[chan].calc_pwm(functions[function].output_scaled);
return channels[chan].get_output_norm();
}
/*
limit slew rate for an output function to given rate in percent per second
*/
void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate)
{
// NOT IMPLEMENTED YET
}
// call set_angle() on matching channels
void SRV_Channels::set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_angle(angle);
}
}
}
// call set_range() on matching channels
void SRV_Channels::set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range)
{
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if (channels[i].function == function) {
channels[i].set_range(range);
}
}
}