mirror of https://github.com/ArduPilot/ardupilot
442 lines
11 KiB
C++
442 lines
11 KiB
C++
/*
|
|
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/>.
|
|
*/
|
|
|
|
/*
|
|
* RC_Channel.cpp - Radio library for Arduino
|
|
* Code by Jason Short. DIYDrones.com
|
|
*
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <cmath>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include "RC_Channel.h"
|
|
|
|
RC_Channel *RC_Channels::channels;
|
|
|
|
const AP_Param::GroupInfo RC_Channel::var_info[] = {
|
|
// @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: pwm
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("MIN", 1, 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: pwm
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("TRIM", 2, 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: pwm
|
|
// @Range: 800 2200
|
|
// @Increment: 1
|
|
// @User: Advanced
|
|
AP_GROUPINFO("MAX", 3, RC_Channel, radio_max, 1900),
|
|
|
|
// @Param: REVERSED
|
|
// @DisplayName: RC reversed
|
|
// @Description: Reverse servo operation. Set to 0 for normal (forward) operation. Set to 1 to reverse this input channel.
|
|
// @Values: 0:Normal,1:Reversed
|
|
// @User: Advanced
|
|
AP_GROUPINFO("REVERSED", 4, RC_Channel, reversed, 0),
|
|
|
|
// @Param: DZ
|
|
// @DisplayName: RC dead-zone
|
|
// @Description: dead zone around trim or bottom
|
|
// @Units: pwm
|
|
// @Range: 0 200
|
|
// @User: Advanced
|
|
AP_GROUPINFO("DZ", 5, RC_Channel, dead_zone, 0),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
|
|
const AP_Param::GroupInfo RC_Channels::var_info[] = {
|
|
// @Group: 1_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[0], "1_", 1, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 2_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[1], "2_", 2, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 3_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[2], "3_", 3, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 4_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[3], "4_", 4, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 5_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[4], "5_", 5, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 6_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[5], "6_", 6, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 7_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[6], "7_", 7, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 8_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[7], "8_", 8, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 9_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[8], "9_", 9, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 10_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[9], "10_", 10, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 11_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[10], "11_", 11, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 12_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[11], "12_", 12, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 13_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[12], "13_", 13, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 14_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[13], "14_", 14, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 15_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[14], "15_", 15, RC_Channels, RC_Channel),
|
|
|
|
// @Group: 16_
|
|
// @Path: RC_Channel.cpp
|
|
AP_SUBGROUPINFO(obj_channels[15], "16_", 16, RC_Channels, RC_Channel),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
|
|
// constructor
|
|
RC_Channel::RC_Channel(void)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
}
|
|
|
|
/*
|
|
channels group object constructor
|
|
*/
|
|
RC_Channels::RC_Channels(void)
|
|
{
|
|
channels = obj_channels;
|
|
|
|
// set defaults from the parameter table
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
// setup ch_in on channels
|
|
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
|
|
channels[i].ch_in = i;
|
|
}
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_range(uint16_t high)
|
|
{
|
|
type_in = RC_CHANNEL_TYPE_RANGE;
|
|
high_in = high;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_angle(uint16_t angle)
|
|
{
|
|
type_in = RC_CHANNEL_TYPE_ANGLE;
|
|
high_in = angle;
|
|
}
|
|
|
|
void
|
|
RC_Channel::set_default_dead_zone(int16_t dzone)
|
|
{
|
|
dead_zone.set_default(abs(dzone));
|
|
}
|
|
|
|
bool
|
|
RC_Channel::get_reverse(void) const
|
|
{
|
|
return bool(reversed.get());
|
|
}
|
|
|
|
// read input from APM_RC - create a control_in value
|
|
void
|
|
RC_Channel::set_pwm(int16_t pwm)
|
|
{
|
|
radio_in = pwm;
|
|
|
|
if (type_in == RC_CHANNEL_TYPE_RANGE) {
|
|
control_in = pwm_to_range();
|
|
} else {
|
|
//RC_CHANNEL_TYPE_ANGLE
|
|
control_in = pwm_to_angle();
|
|
}
|
|
}
|
|
|
|
/*
|
|
call read() and set_pwm() on all channels
|
|
*/
|
|
void
|
|
RC_Channels::set_pwm_all(void)
|
|
{
|
|
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
|
|
channels[i].set_pwm(channels[i].read());
|
|
}
|
|
}
|
|
|
|
// 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_in == RC_CHANNEL_TYPE_RANGE) {
|
|
control_in = pwm_to_range_dz(0);
|
|
} else {
|
|
//RC_CHANNEL_ANGLE
|
|
control_in = pwm_to_angle_dz(0);
|
|
}
|
|
}
|
|
|
|
/*
|
|
return the center stick position expressed as a control_in value
|
|
used for thr_mid in copter
|
|
*/
|
|
int16_t RC_Channel::get_control_mid() const
|
|
{
|
|
if (type_in == RC_CHANNEL_TYPE_RANGE) {
|
|
int16_t r_in = (radio_min.get() + radio_max.get())/2;
|
|
|
|
if (reversed) {
|
|
r_in = radio_max.get() - (r_in - radio_min.get());
|
|
}
|
|
|
|
int16_t radio_trim_low = radio_min + dead_zone;
|
|
|
|
return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// ------------------------------------------
|
|
|
|
void RC_Channel::load_eeprom(void)
|
|
{
|
|
radio_min.load();
|
|
radio_trim.load();
|
|
radio_max.load();
|
|
reversed.load();
|
|
dead_zone.load();
|
|
}
|
|
|
|
void RC_Channel::save_eeprom(void)
|
|
{
|
|
radio_min.save();
|
|
radio_trim.save();
|
|
radio_max.save();
|
|
reversed.save();
|
|
dead_zone.save();
|
|
}
|
|
|
|
/*
|
|
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_trim(uint16_t _dead_zone, uint16_t _trim)
|
|
{
|
|
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)
|
|
return 0;
|
|
|
|
int16_t reverse_mul = (reversed?-1:1);
|
|
if (radio_in > radio_trim_high) {
|
|
return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_high)) / (int32_t)(radio_max - radio_trim_high);
|
|
} else if (radio_in < radio_trim_low) {
|
|
return reverse_mul * ((int32_t)high_in * (int32_t)(radio_in - radio_trim_low)) / (int32_t)(radio_trim_low - radio_min);
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/*
|
|
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)
|
|
{
|
|
return pwm_to_angle_dz_trim(_dead_zone, radio_trim);
|
|
}
|
|
|
|
/*
|
|
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);
|
|
}
|
|
|
|
|
|
/*
|
|
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 (reversed) {
|
|
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 (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
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::get_control_in_zero_dz(void)
|
|
{
|
|
if (type_in == RC_CHANNEL_TYPE_RANGE) {
|
|
return pwm_to_range_dz(0);
|
|
}
|
|
return pwm_to_angle_dz(0);
|
|
}
|
|
|
|
// ------------------------------------------
|
|
|
|
float
|
|
RC_Channel::norm_input()
|
|
{
|
|
float ret;
|
|
int16_t reverse_mul = (reversed?-1:1);
|
|
if (radio_in < radio_trim) {
|
|
if (radio_min >= radio_trim) {
|
|
return 0.0f;
|
|
}
|
|
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
|
|
} else {
|
|
if (radio_max <= radio_trim) {
|
|
return 0.0f;
|
|
}
|
|
ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
|
|
}
|
|
return constrain_float(ret, -1.0f, 1.0f);
|
|
}
|
|
|
|
float
|
|
RC_Channel::norm_input_dz()
|
|
{
|
|
int16_t dz_min = radio_trim - dead_zone;
|
|
int16_t dz_max = radio_trim + dead_zone;
|
|
float ret;
|
|
int16_t reverse_mul = (reversed?-1:1);
|
|
if (radio_in < dz_min && dz_min > radio_min) {
|
|
ret = reverse_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min);
|
|
} else if (radio_in > dz_max && radio_max > dz_max) {
|
|
ret = reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max);
|
|
} else {
|
|
ret = 0;
|
|
}
|
|
return constrain_float(ret, -1.0f, 1.0f);
|
|
}
|
|
|
|
/*
|
|
get percentage input from 0 to 100. This ignores the trim value.
|
|
*/
|
|
uint8_t
|
|
RC_Channel::percent_input()
|
|
{
|
|
if (radio_in <= radio_min) {
|
|
return reversed?100:0;
|
|
}
|
|
if (radio_in >= radio_max) {
|
|
return reversed?0:100;
|
|
}
|
|
uint8_t ret = 100.0f * (radio_in - radio_min) / (float)(radio_max - radio_min);
|
|
if (reversed) {
|
|
ret = 100 - ret;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void
|
|
RC_Channel::input()
|
|
{
|
|
radio_in = hal.rcin->read(ch_in);
|
|
}
|
|
|
|
uint16_t
|
|
RC_Channel::read() const
|
|
{
|
|
return hal.rcin->read(ch_in);
|
|
}
|
|
|
|
/*
|
|
Return true if the channel is at trim and within the DZ
|
|
*/
|
|
bool RC_Channel::in_trim_dz()
|
|
{
|
|
return is_bounded_int32(radio_in, radio_trim - dead_zone, radio_trim + dead_zone);
|
|
}
|
|
|