Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/RC_Channel/SRV_Channel.cpp

314 lines
10 KiB
C++
Raw Blame History

/*
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 "SRV_Channel.h"
#include "RC_Channel.h"
extern const AP_HAL::HAL& hal;
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),
// @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", 18, SRV_Channels, auto_trim, 0),
AP_GROUPEND
};
/*
constructor
*/
SRV_Channels::SRV_Channels(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
// remap a PWM value from a channel in value
uint16_t SRV_Channels::remap_pwm(uint8_t i, uint16_t pwm) const
{
const RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == nullptr) {
return 0;
}
float v = ch->get_radio_out_normalised(pwm);
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]);
}
}
radio_out = constrain_int16(radio_out, servo_min[i], servo_max[i]);
return radio_out;
}
/*
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)
{
// cope with SERVO_RNG_ENABLE being changed at runtime. If we
// don't change the ESC scaling immediately then some ESCs will
// fire up for one second
if (last_enable != enable && esc_cal_chan != -1) {
last_enable = enable;
set_esc_scaling(esc_cal_chan);
}
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;
}
uint16_t radio_out = remap_pwm(i, ch->get_radio_out());
ch->set_radio_out(radio_out);
}
}
/*
set trim values for output channels
*/
void SRV_Channels::set_trim(void)
{
if (!enable) {
return;
}
for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
const RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == nullptr) {
continue;
}
if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
// we don't trim range channels (like throttle)
continue;
}
uint16_t new_trim = remap_pwm(i, ch->get_radio_trim());
servo_trim[i].set_and_save(new_trim);
}
}
void SRV_Channels::set_esc_scaling(uint8_t chnum)
{
esc_cal_chan = chnum;
if (!enable || chnum >= NUM_SERVO_RANGE_CHANNELS) {
const RC_Channel *ch = RC_Channel::rc_channel(chnum);
hal.rcout->set_esc_scaling(ch->get_radio_min(), ch->get_radio_max());
} else {
hal.rcout->set_esc_scaling(servo_min[chnum], servo_max[chnum]);
}
}
/*
auto-adjust channel trim from an integrator value. Positive v means
adjust trim up. Negative means decrease
*/
void SRV_Channels::adjust_trim(uint8_t chnum, float v)
{
if (reverse[chnum] == -1) {
v = -v;
}
uint16_t new_trim = servo_trim[chnum];
float trim_scaled = float(servo_trim[chnum] - servo_min[chnum]) / (servo_max[chnum] - servo_min[chnum]);
if (v > 0 && trim_scaled < 0.6f) {
new_trim++;
} else if (v < 0 && trim_scaled > 0.4f) {
new_trim--;
} else {
return;
}
servo_trim[chnum].set(new_trim);
trimmed_mask |= 1U<<chnum;
}
/*
save adjusted trims
*/
void SRV_Channels::save_trim(void)
{
for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
if (trimmed_mask & (1U<<i)) {
servo_trim[i].set_and_save(servo_trim[i].get());
}
}
trimmed_mask = 0;
}
Reference in New Issue View Git Blame Copy Permalink