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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@926 f9c3cf11-9bcb-44bc-f272-b75c42450872

Browse Source
This commit is contained in:
jasonshort 2010-11-25 07:10:06 +00:00
parent 7582353b08
commit dc16a2961d
3 changed files with 65 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/RC_Channel/RC_Channel.cpp
View File

@ -18,13 +18,6 @@ RC_Channel::RC_Channel() : _high(1)
{
}
void
RC_Channel::set_radio_range(int r_min, int r_max)
{
_radio_min = r_min;
_radio_max = r_max;
}
// setup the control preferences
void
RC_Channel::set_range(int low, int high)
@ -43,9 +36,10 @@ RC_Channel::set_angle(int angle)
// call after first read
void
RC_Channel::set_trim(int pwm)
RC_Channel::trim()
{
_radio_trim = pwm;
radio_trim = radio_in;
}
// read input from APM_RC - create a control_in value
@ -54,16 +48,18 @@ RC_Channel::set_pwm(int pwm)
{
//Serial.print(pwm,DEC);
if(_radio_in == 0)
_radio_in = pwm;
if(radio_in == 0)
radio_in = pwm;
else
_radio_in = ((pwm + _radio_in) >> 1); // Small filtering
radio_in = ((pwm + radio_in) >> 1); // Small filtering
if(_type == RANGE){
//Serial.print("range ");
control_in = pwm_to_range();
control_in = (control_in < dead_zone) ? 0 : control_in;
}else{
control_in = pwm_to_angle();
control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
}
}
@ -71,7 +67,7 @@ RC_Channel::set_pwm(int pwm)
boolean
RC_Channel::get_failsafe(void)
{
return (_radio_in < (_radio_min - 50));
return (radio_in < (radio_min - 50));
}
// returns just the PWM without the offset from radio_min
@ -84,47 +80,50 @@ RC_Channel::calc_pwm(void)
}else{
pwm_out = angle_to_pwm();
}
radio_out = pwm_out + _radio_min;
radio_out = pwm_out + radio_min;
}
void
RC_Channel::load_eeprom(int address)
{
//Serial.println("load gains ");
//Serial.println(address, DEC);
//_kp = (float)(eeprom_read_word((uint16_t *) address)) / 1000.0;
//_ki = (float)(eeprom_read_word((uint16_t *) (address + 2))) / 1000.0;
//_kd = (float)(eeprom_read_word((uint16_t *) (address + 4))) / 1000.0;
//_imax = eeprom_read_word((uint16_t *) (address + 6)) * 100;
radio_min = eeprom_read_word((uint16_t *) address);
radio_max = eeprom_read_word((uint16_t *) (address + 2));
radio_trim = eeprom_read_word((uint16_t *) (address + 4));
}
void
RC_Channel::save_eeprom(int address)
{
//eeprom_write_word((uint16_t *) address, (int)(_kp * 1000));
//eeprom_write_word((uint16_t *) (address + 2), (int)(_ki * 1000));
//eeprom_write_word((uint16_t *) (address + 4), (int)(_kd * 1000));
//eeprom_write_word((uint16_t *) (address + 6), (int)_imax/100);
eeprom_write_word((uint16_t *) address, radio_min);
eeprom_write_word((uint16_t *) (address + 2), radio_max);
eeprom_write_word((uint16_t *) (address + 4), radio_trim);
}
// ------------------------------------------
void
RC_Channel::update_min_max()
{
radio_min = min(radio_min, radio_in);
radio_max = min(radio_max, radio_in);
}
// ------------------------------------------
int16_t
RC_Channel::pwm_to_angle()
{
if(_radio_in < _radio_trim)
return _high * ((float)(_radio_in - _radio_trim) / (float)(_radio_trim - _radio_min));
if(radio_in < radio_trim)
return _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
else
return _high * ((float)(_radio_in - _radio_trim) / (float)(_radio_max - _radio_trim));
return _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim));
}
int16_t
RC_Channel::angle_to_pwm()
{
if(servo_out < 0)
return (((float)servo_out / (float)_high) * (float)(_radio_max - _radio_trim));
return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
else
return (((float)servo_out / (float)_high) * (float)(_radio_trim - _radio_min));
return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
}
// ------------------------------------------
@ -132,12 +131,12 @@ RC_Channel::angle_to_pwm()
int16_t
RC_Channel::pwm_to_range()
{
return _low + _high * ((float)(_radio_in - _radio_min) / (float)(_radio_max - _radio_min));
return _low + _high * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min));
}
int16_t
RC_Channel::range_to_pwm()
{
return (((float)servo_out / (float)(_high - _low)) * (float)(_radio_max - _radio_min));
return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min));
}

23
libraries/RC_Channel/RC_Channel.h
View File

@ -8,6 +8,7 @@
#define RC_Channel_h
#include <inttypes.h>
#include <avr/eeprom.h>
#include "WProgram.h"
class RC_Channel
@ -15,26 +16,32 @@ class RC_Channel
public:
RC_Channel();
// setup in CLI
void update_min_max();
// startup
void load_eeprom(int address);
void save_eeprom(int address);
void set_radio_range(int r_min, int r_max);
// setup the control preferences
void set_range(int low, int high);
void set_angle(int angle);
// call after first read
void set_trim(int pwm);
// read input from APM_RC - create a control_in value
void set_pwm(int pwm);
// pwm is stored here
int16_t radio_in;
// call after first set_pwm
void trim();
// did our read come in 50µs below the min?
boolean get_failsafe(void);
// value generated from PWM
int16_t control_in;
int8_t dead_zone; // used to keep noise down and create a dead zone.
// current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100
int16_t servo_out;
@ -45,6 +52,10 @@ class RC_Channel
// PWM is without the offset from radio_min
int16_t pwm_out;
int16_t radio_out;
int16_t radio_min;
int16_t radio_trim;
int16_t radio_max;
// includes offset from PWM
//int16_t get_radio_out(void);
@ -60,10 +71,6 @@ class RC_Channel
boolean _type;
boolean _filter;
int16_t _radio_in;
int16_t _radio_min;
int16_t _radio_trim;
int16_t _radio_max;
int16_t _high;
int16_t _low;

29
libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde
View File

@ -28,10 +28,17 @@ void setup()
// setup radio
// read eepom or set manually
rc_1.set_radio_range(1100,1900);
rc_2.set_radio_range(1100,1900);
rc_3.set_radio_range(1100,1900);
rc_4.set_radio_range(1100,1900);
rc_1.radio_min = 1100;
rc_1.radio_max = 1900;
rc_2.radio_min = 1100;
rc_2.radio_max = 1900;
rc_3.radio_min = 1100;
rc_3.radio_max = 1900;
rc_4.radio_min = 1100;
rc_4.radio_max = 1900;
// set type of output, symmetrical angles or a number range;
rc_1.set_angle(4500);
@ -40,11 +47,15 @@ void setup()
rc_4.set_angle(3000);
// set midpoint value
rc_1.set_trim(APM_RC.InputCh(CH_1));
rc_2.set_trim(APM_RC.InputCh(CH_2));
rc_3.set_trim(APM_RC.InputCh(CH_3));
rc_4.set_trim(APM_RC.InputCh(CH_4));
rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_1.trim();
rc_2.trim();
rc_3.trim();
rc_4.trim();
}
void loop()