mirror of https://github.com/ArduPilot/ardupilot
RC_Channel: added SRV_Channels object for remapping output channels
this allows for decoupling RC input ranges from servo output ranges
This commit is contained in:
Andrew Tridgell
parent
fea7903aa8
commit
c7f8b255e7
|
|
@ -0,0 +1,216 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
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 "SRV_Channel.h"
|
||||
#include "RC_Channel.h"
|
||||
|
||||
const AP_Param::GroupInfo SRV_Channels::var_info[] = {
|
||||
// @Param: RNG_ENABLE
|
||||
// @DisplayName: Enable servo output ranges
|
||||
// @Description: This enables the use of separate servo output ranges from input ranges.
|
||||
// @Values: 0:Disable,1:Enable
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("_RNG_ENABLE", 1, SRV_Channels, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: 1_MIN
|
||||
// @DisplayName: Servo1 min PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("1_MIN", 2, SRV_Channels, servo_min[0], 1100),
|
||||
|
||||
// @Param: 1_MAX
|
||||
// @DisplayName: Servo1 max PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("1_MAX", 3, SRV_Channels, servo_max[0], 1900),
|
||||
|
||||
// @Param: 1_TRIM
|
||||
// @DisplayName: Servo1 trim PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("1_TRIM", 4, SRV_Channels, servo_trim[0], 1500),
|
||||
|
||||
// @Param: 1_REV
|
||||
// @DisplayName: Servo1 reverse
|
||||
// @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
|
||||
// @Values: -1:Reversed,1:Normal
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("1_REV", 5, SRV_Channels, reverse[0], 1),
|
||||
|
||||
// @Param: 2_MIN
|
||||
// @DisplayName: Servo1 min PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("2_MIN", 6, SRV_Channels, servo_min[1], 1100),
|
||||
|
||||
// @Param: 2_MAX
|
||||
// @DisplayName: Servo1 max PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("2_MAX", 7, SRV_Channels, servo_max[1], 1900),
|
||||
|
||||
// @Param: 2_TRIM
|
||||
// @DisplayName: Servo1 trim PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("2_TRIM", 8, SRV_Channels, servo_trim[1], 1500),
|
||||
|
||||
// @Param: 2_REV
|
||||
// @DisplayName: Servo1 reverse
|
||||
// @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
|
||||
// @Values: -1:Reversed,1:Normal
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("2_REV", 9, SRV_Channels, reverse[1], 1),
|
||||
|
||||
// @Param: 3_MIN
|
||||
// @DisplayName: Servo1 min PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("3_MIN", 10, SRV_Channels, servo_min[2], 1100),
|
||||
|
||||
// @Param: 3_MAX
|
||||
// @DisplayName: Servo1 max PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("3_MAX", 11, SRV_Channels, servo_max[2], 1900),
|
||||
|
||||
// @Param: 3_TRIM
|
||||
// @DisplayName: Servo1 trim PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("3_TRIM", 12, SRV_Channels, servo_trim[2], 1500),
|
||||
|
||||
// @Param: 3_REV
|
||||
// @DisplayName: Servo1 reverse
|
||||
// @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
|
||||
// @Values: -1:Reversed,1:Normal
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("3_REV", 13, SRV_Channels, reverse[2], 1),
|
||||
|
||||
// @Param: 4_MIN
|
||||
// @DisplayName: Servo1 min PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("4_MIN", 14, SRV_Channels, servo_min[3], 1100),
|
||||
|
||||
// @Param: 4_MAX
|
||||
// @DisplayName: Servo1 max PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("4_MAX", 15, SRV_Channels, servo_max[3], 1900),
|
||||
|
||||
// @Param: 4_TRIM
|
||||
// @DisplayName: Servo1 trim PWM
|
||||
// @Description: servo1 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: Advanced
|
||||
AP_GROUPINFO("4_TRIM", 16, SRV_Channels, servo_trim[3], 1500),
|
||||
|
||||
// @Param: 4_REV
|
||||
// @DisplayName: Servo1 reverse
|
||||
// @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
|
||||
// @Values: -1:Reversed,1:Normal
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("4_REV", 17, SRV_Channels, reverse[3], 1),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
constructor
|
||||
*/
|
||||
SRV_Channels::SRV_Channels(void)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
remap radio_out values for first 4 servos using SERVO* parameters, if enabled
|
||||
This should be called with cork enabled in hal.rcout
|
||||
*/
|
||||
void SRV_Channels::remap_servo_output(void)
|
||||
{
|
||||
if (!enable) {
|
||||
return;
|
||||
}
|
||||
for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
|
||||
RC_Channel *ch = RC_Channel::rc_channel(i);
|
||||
if (ch == nullptr) {
|
||||
continue;
|
||||
}
|
||||
float v = ch->get_radio_out_normalised();
|
||||
uint16_t radio_out;
|
||||
if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
|
||||
if (reverse[i] == -1) {
|
||||
v = 1 - v;
|
||||
}
|
||||
radio_out = servo_min[i] + v * (servo_max[i] - servo_min[i]);
|
||||
} else {
|
||||
if (reverse[i] == -1) {
|
||||
v = -v;
|
||||
}
|
||||
if (v > 0) {
|
||||
radio_out = servo_trim[i] + v * (servo_max[i] - servo_trim[i]);
|
||||
} else {
|
||||
radio_out = servo_trim[i] + v * (servo_trim[i] - servo_min[i]);
|
||||
}
|
||||
}
|
||||
ch->set_radio_out(radio_out);
|
||||
ch->output();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
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>
|
||||
|
||||
#define NUM_SERVO_RANGE_CHANNELS 4
|
||||
|
||||
/*
|
||||
class SRV_Channel
|
||||
*/
|
||||
class SRV_Channels {
|
||||
public:
|
||||
// constructor
|
||||
SRV_Channels(void);
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// take current radio_out for first 4 channels and remap using
|
||||
// servo ranges if enabled
|
||||
void remap_servo_output(void);
|
||||
|
||||
private:
|
||||
AP_Int8 enable;
|
||||
|
||||
// PWM values for min/max and trim
|
||||
AP_Int16 servo_min[NUM_SERVO_RANGE_CHANNELS];
|
||||
AP_Int16 servo_max[NUM_SERVO_RANGE_CHANNELS];
|
||||
AP_Int16 servo_trim[NUM_SERVO_RANGE_CHANNELS];
|
||||
|
||||
// reversal, following convention that < 0 means reversed, >= 0 means normal
|
||||
AP_Int8 reverse[NUM_SERVO_RANGE_CHANNELS];
|
||||
};
|
||||
Loading…
Reference in New Issue