mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
21859f9364
The dead_zone was being used inconsistently, used in PWM space in one place, and in control output space in another. The fix required us to move the index number of the RC channel eeprom value for RCn_DZ, as users will have a throttle deadzone of 3 set in their eeprom due to a bug that Randy just fixed that caused the value to always be saved to eeprom. In plane we then need to fix the deadzone for the throttle to be 30 this fixes issue #303 Thanks to Soren Kuula for spotting this!
373 lines
8.8 KiB
C++
373 lines
8.8 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
* RC_Channel.cpp - Radio library for Arduino
|
|
* Code by Jason Short. DIYDrones.com
|
|
*
|
|
* This library is free software; you can redistribute it and / or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#include <AP_HAL.h>
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <AP_Math.h>
|
|
|
|
#include "RC_Channel.h"
|
|
|
|
#define NUM_CHANNELS 8
|
|
/// global array with pointers to all APM RC channels, will be used by AP_Mount
|
|
/// and AP_Camera classes / It points to RC input channels, both APM1 and APM2
|
|
/// only have 8 input channels.
|
|
RC_Channel *RC_Channel::rc_ch[NUM_CHANNELS];
|
|
|
|
const AP_Param::GroupInfo RC_Channel::var_info[] PROGMEM = {
|
|
// @Param: MIN
|
|
// @DisplayName: RC min PWM
|
|
// @Description: RC minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
|
|
// @Units: ms
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("MIN", 0, RC_Channel, radio_min, 1100),
|
|
|
|
// @Param: TRIM
|
|
// @DisplayName: RC trim PWM
|
|
// @Description: RC trim (neutral) PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
|
|
// @Units: ms
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("TRIM", 1, RC_Channel, radio_trim, 1500),
|
|
|
|
// @Param: MAX
|
|
// @DisplayName: RC max PWM
|
|
// @Description: RC maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
|
|
// @Units: ms
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("MAX", 2, RC_Channel, radio_max, 1900),
|
|
|
|
// @Param: REV
|
|
// @DisplayName: RC 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("REV", 3, RC_Channel, _reverse, 1),
|
|
|
|
// Note: index 4 was used by the previous _dead_zone value. We
|
|
// changed it to 5 as dead zone values had previously been
|
|
// incorrectly saved, overriding user values. They were also
|
|
// incorrectly interpreted for the throttle on APM:Plane
|
|
|
|
// @Param: DZ
|
|
// @DisplayName: RC dead-zone
|
|
// @Description: dead zone around trim.
|
|
// @User: Advanced
|
|
AP_GROUPINFO("DZ", 5, RC_Channel, _dead_zone, 0),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
// setup the control preferences
|
|
void
|
|
RC_Channel::set_range(int16_t low, int16_t high)
|
|
{
|
|
_type = RC_CHANNEL_TYPE_RANGE;
|
|
_high = high;
|
|
_low = low;
|
|
_high_out = high;
|
|
_low_out = low;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_range_out(int16_t low, int16_t high)
|
|
{
|
|
_high_out = high;
|
|
_low_out = low;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_angle(int16_t angle)
|
|
{
|
|
_type = RC_CHANNEL_TYPE_ANGLE;
|
|
_high = angle;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_default_dead_zone(int16_t dzone)
|
|
{
|
|
if (!_dead_zone.load()) {
|
|
_dead_zone.set(abs(dzone));
|
|
}
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_reverse(bool reverse)
|
|
{
|
|
if (reverse) _reverse = -1;
|
|
else _reverse = 1;
|
|
}
|
|
|
|
bool
|
|
RC_Channel::get_reverse(void)
|
|
{
|
|
if (_reverse==-1) return 1;
|
|
else return 0;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_type(uint8_t t)
|
|
{
|
|
_type = t;
|
|
}
|
|
|
|
// call after first read
|
|
void
|
|
RC_Channel::trim()
|
|
{
|
|
radio_trim = radio_in;
|
|
}
|
|
|
|
// read input from APM_RC - create a control_in value
|
|
void
|
|
RC_Channel::set_pwm(int16_t pwm)
|
|
{
|
|
radio_in = pwm;
|
|
|
|
if (_type == RC_CHANNEL_TYPE_RANGE) {
|
|
control_in = pwm_to_range();
|
|
} else {
|
|
//RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
|
|
control_in = pwm_to_angle();
|
|
}
|
|
}
|
|
|
|
// read input from APM_RC - create a control_in value, but use a
|
|
// zero value for the dead zone. When done this way the control_in
|
|
// value can be used as servo_out to give the same output as input
|
|
void
|
|
RC_Channel::set_pwm_no_deadzone(int16_t pwm)
|
|
{
|
|
radio_in = pwm;
|
|
|
|
if (_type == RC_CHANNEL_TYPE_RANGE) {
|
|
control_in = pwm_to_range_dz(0);
|
|
} else {
|
|
//RC_CHANNEL_ANGLE, RC_CHANNEL_ANGLE_RAW
|
|
control_in = pwm_to_angle_dz(0);
|
|
}
|
|
}
|
|
|
|
int16_t
|
|
RC_Channel::control_mix(float value)
|
|
{
|
|
return (1 - abs(control_in / _high)) * value + control_in;
|
|
}
|
|
|
|
// are we below a threshold?
|
|
bool
|
|
RC_Channel::get_failsafe(void)
|
|
{
|
|
return (radio_in < (radio_min - 50));
|
|
}
|
|
|
|
// returns just the PWM without the offset from radio_min
|
|
void
|
|
RC_Channel::calc_pwm(void)
|
|
{
|
|
if(_type == RC_CHANNEL_TYPE_RANGE) {
|
|
pwm_out = range_to_pwm();
|
|
radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out);
|
|
|
|
}else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) {
|
|
pwm_out = (float)servo_out * 0.1f;
|
|
radio_out = (pwm_out * _reverse) + radio_trim;
|
|
|
|
}else{ // RC_CHANNEL_TYPE_ANGLE
|
|
pwm_out = angle_to_pwm();
|
|
radio_out = pwm_out + radio_trim;
|
|
}
|
|
|
|
radio_out = constrain_int16(radio_out, radio_min.get(), radio_max.get());
|
|
}
|
|
|
|
// ------------------------------------------
|
|
|
|
void
|
|
RC_Channel::load_eeprom(void)
|
|
{
|
|
radio_min.load();
|
|
radio_trim.load();
|
|
radio_max.load();
|
|
_reverse.load();
|
|
_dead_zone.load();
|
|
}
|
|
|
|
void
|
|
RC_Channel::save_eeprom(void)
|
|
{
|
|
radio_min.save();
|
|
radio_trim.save();
|
|
radio_max.save();
|
|
_reverse.save();
|
|
_dead_zone.save();
|
|
}
|
|
|
|
// ------------------------------------------
|
|
|
|
void
|
|
RC_Channel::zero_min_max()
|
|
{
|
|
radio_min = radio_max = radio_in;
|
|
}
|
|
|
|
void
|
|
RC_Channel::update_min_max()
|
|
{
|
|
radio_min = min(radio_min.get(), radio_in);
|
|
radio_max = max(radio_max.get(), radio_in);
|
|
}
|
|
|
|
/*
|
|
return an "angle in centidegrees" (normally -4500 to 4500) from
|
|
the current radio_in value using the specified dead_zone
|
|
*/
|
|
int16_t
|
|
RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
|
|
{
|
|
int16_t radio_trim_high = radio_trim + dead_zone;
|
|
int16_t radio_trim_low = radio_trim - dead_zone;
|
|
|
|
// prevent div by 0
|
|
if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
|
|
return 0;
|
|
|
|
if(radio_in > radio_trim_high) {
|
|
return _reverse * ((long)_high * (long)(radio_in - radio_trim_high)) / (long)(radio_max - radio_trim_high);
|
|
}else if(radio_in < radio_trim_low) {
|
|
return _reverse * ((long)_high * (long)(radio_in - radio_trim_low)) / (long)(radio_trim_low - radio_min);
|
|
}else
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
return an "angle in centidegrees" (normally -4500 to 4500) from
|
|
the current radio_in value
|
|
*/
|
|
int16_t
|
|
RC_Channel::pwm_to_angle()
|
|
{
|
|
return pwm_to_angle_dz(_dead_zone);
|
|
}
|
|
|
|
|
|
int16_t
|
|
RC_Channel::angle_to_pwm()
|
|
{
|
|
if((servo_out * _reverse) > 0)
|
|
return _reverse * ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high;
|
|
else
|
|
return _reverse * ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high;
|
|
}
|
|
|
|
/*
|
|
convert a pulse width modulation value to a value in the configured
|
|
range, using the specified deadzone
|
|
*/
|
|
int16_t
|
|
RC_Channel::pwm_to_range_dz(uint16_t dead_zone)
|
|
{
|
|
int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());
|
|
|
|
if (_reverse == -1) {
|
|
r_in = radio_max.get() - (r_in - radio_min.get());
|
|
}
|
|
|
|
int16_t radio_trim_low = radio_min + dead_zone;
|
|
|
|
if (r_in > radio_trim_low)
|
|
return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
|
|
else if (dead_zone > 0)
|
|
return 0;
|
|
else
|
|
return _low;
|
|
}
|
|
|
|
/*
|
|
convert a pulse width modulation value to a value in the configured
|
|
range
|
|
*/
|
|
int16_t
|
|
RC_Channel::pwm_to_range()
|
|
{
|
|
return pwm_to_range_dz(_dead_zone);
|
|
}
|
|
|
|
|
|
int16_t
|
|
RC_Channel::range_to_pwm()
|
|
{
|
|
return ((long)(servo_out - _low_out) * (long)(radio_max - radio_min)) / (long)(_high_out - _low_out);
|
|
}
|
|
|
|
// ------------------------------------------
|
|
|
|
float
|
|
RC_Channel::norm_input()
|
|
{
|
|
if(radio_in < radio_trim)
|
|
return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
|
|
else
|
|
return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
|
|
}
|
|
|
|
float
|
|
RC_Channel::norm_output()
|
|
{
|
|
int16_t mid = (radio_max + radio_min) / 2;
|
|
float ret;
|
|
if(radio_out < mid)
|
|
ret = (float)(radio_out - mid) / (float)(mid - radio_min);
|
|
else
|
|
ret = (float)(radio_out - mid) / (float)(radio_max - mid);
|
|
if (_reverse == -1) {
|
|
ret = -ret;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void RC_Channel::output() const
|
|
{
|
|
hal.rcout->write(_ch_out, radio_out);
|
|
}
|
|
|
|
void RC_Channel::output_trim() const
|
|
{
|
|
hal.rcout->write(_ch_out, radio_trim);
|
|
}
|
|
|
|
void
|
|
RC_Channel::input()
|
|
{
|
|
radio_in = hal.rcin->read(_ch_out);
|
|
}
|
|
|
|
uint16_t
|
|
RC_Channel::read() const
|
|
{
|
|
return hal.rcin->read(_ch_out);
|
|
}
|
|
|
|
void
|
|
RC_Channel::enable_out()
|
|
{
|
|
hal.rcout->enable_ch(_ch_out);
|
|
}
|