This is a new Library for managing RC input channels which will replace the current way of storing RC input in Arrays and most of the Radio.pde tab
git-svn-id: https://arducopter.googlecode.com/svn/trunk@902 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort@gmail.com
parent
f30bf9cab7
commit
4fa2491adf
140
libraries/RC_Channel/RC_Channel.cpp
Normal file
140
libraries/RC_Channel/RC_Channel.cpp
Normal file
@ -0,0 +1,140 @@
|
||||
/*
|
||||
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 "RC_Channel.h"
|
||||
|
||||
#define ANGLE 0
|
||||
#define RANGE 1
|
||||
|
||||
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 high, int low)
|
||||
{
|
||||
_type = RANGE;
|
||||
_high = high;
|
||||
_low = low;
|
||||
}
|
||||
|
||||
void
|
||||
RC_Channel::set_angle(int angle)
|
||||
{
|
||||
_type = ANGLE;
|
||||
_high = angle;
|
||||
}
|
||||
|
||||
// call after first read
|
||||
void
|
||||
RC_Channel::set_trim(int pwm)
|
||||
{
|
||||
_radio_trim = pwm;
|
||||
}
|
||||
|
||||
// read input from APM_RC - create a control_in value
|
||||
void
|
||||
RC_Channel::set_pwm(int pwm)
|
||||
{
|
||||
if(_radio_in == 0)
|
||||
_radio_in = pwm;
|
||||
else
|
||||
_radio_in = ((pwm + _radio_in) >> 1); // Small filtering
|
||||
|
||||
if(_type == RANGE){
|
||||
control_in = pwm_to_range();
|
||||
}else{
|
||||
control_in = pwm_to_angle();
|
||||
}
|
||||
}
|
||||
|
||||
// are we below a threshold?
|
||||
boolean
|
||||
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 == RANGE){
|
||||
pwm_out = range_to_pwm();
|
||||
}else{
|
||||
pwm_out = angle_to_pwm();
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// ------------------------------------------
|
||||
|
||||
int16_t
|
||||
RC_Channel::pwm_to_angle()
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
int16_t
|
||||
RC_Channel::angle_to_pwm()
|
||||
{
|
||||
if(servo_out < 0)
|
||||
return (((float)servo_out / (float)_high) * (float)(_radio_max - _radio_trim));
|
||||
else
|
||||
return (((float)servo_out / (float)_high) * (float)(_radio_trim - _radio_min));
|
||||
}
|
||||
|
||||
// ------------------------------------------
|
||||
|
||||
int16_t
|
||||
RC_Channel::pwm_to_range()
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
76
libraries/RC_Channel/RC_Channel.h
Normal file
76
libraries/RC_Channel/RC_Channel.h
Normal file
@ -0,0 +1,76 @@
|
||||
/*
|
||||
RC_Channel library.
|
||||
Code by Jason Short. 2010
|
||||
DIYDrones.com
|
||||
*/
|
||||
|
||||
#ifndef RC_Channel_h
|
||||
#define RC_Channel_h
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "WProgram.h"
|
||||
|
||||
class RC_Channel
|
||||
{
|
||||
public:
|
||||
RC_Channel();
|
||||
|
||||
// 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 high, int low);
|
||||
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);
|
||||
|
||||
// did our read come in 50µs below the min?
|
||||
boolean get_failsafe(void);
|
||||
|
||||
// value generated from PWM
|
||||
int16_t control_in;
|
||||
|
||||
// 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;
|
||||
|
||||
// generate PWM from servo_out value
|
||||
void calc_pwm(void);
|
||||
|
||||
// PWM is without the offset from radio_min
|
||||
int16_t pwm_out;
|
||||
int16_t radio_out;
|
||||
|
||||
// includes offset from PWM
|
||||
//int16_t get_radio_out(void);
|
||||
|
||||
private:
|
||||
int16_t channel_filter();
|
||||
int16_t pwm_to_angle();
|
||||
int16_t angle_to_pwm();
|
||||
int16_t pwm_to_range();
|
||||
int16_t range_to_pwm();
|
||||
|
||||
byte _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;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
70
libraries/RC_Channel/examples/RC_Channel.pde
Normal file
70
libraries/RC_Channel/examples/RC_Channel.pde
Normal file
@ -0,0 +1,70 @@
|
||||
/*
|
||||
Example of RC_Channel library.
|
||||
Code by Jason Short. 2010
|
||||
DIYDrones.com
|
||||
|
||||
*/
|
||||
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <RC_Channel.h> // ArduPilot Mega RC Library
|
||||
|
||||
RC_Channel rc_1;
|
||||
RC_Channel rc_2;
|
||||
RC_Channel rc_3;
|
||||
RC_Channel rc_4;
|
||||
|
||||
#define CH_1 0
|
||||
#define CH_2 1
|
||||
#define CH_3 2
|
||||
#define CH_4 3
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.println("ArduPilot RC Channel test");
|
||||
|
||||
delay(1000);
|
||||
// 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);
|
||||
|
||||
// set type of output, symmetrical angles or a number range;
|
||||
rc_1.set_angle(4500);
|
||||
rc_2.set_angle(4500);
|
||||
rc_3.set_range(0,1000);
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
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));
|
||||
|
||||
print_pwm();
|
||||
}
|
||||
|
||||
void print_pwm()
|
||||
{
|
||||
Serial.print("ch1 ");
|
||||
Serial.print(rc_1.control_in, DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(rc_2.control_in, DEC);
|
||||
Serial.print("\tch3 :");
|
||||
Serial.print(rc_3.control_in, DEC);
|
||||
Serial.print("\tch4 :");
|
||||
Serial.print(rc_4.control_in, DEC);
|
||||
}
|
||||
69
libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde
Normal file
69
libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde
Normal file
@ -0,0 +1,69 @@
|
||||
/*
|
||||
Example of RC_Channel library.
|
||||
Code by Jason Short. 2010
|
||||
DIYDrones.com
|
||||
|
||||
*/
|
||||
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <RC_Channel.h> // ArduPilot Mega RC Library
|
||||
|
||||
RC_Channel rc_1;
|
||||
RC_Channel rc_2;
|
||||
RC_Channel rc_3;
|
||||
RC_Channel rc_4;
|
||||
|
||||
#define CH_1 0
|
||||
#define CH_2 1
|
||||
#define CH_3 2
|
||||
#define CH_4 3
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.println("ArduPilot RC Channel test");
|
||||
|
||||
delay(1000);
|
||||
// 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);
|
||||
|
||||
// set type of output, symmetrical angles or a number range;
|
||||
rc_1.set_angle(4500);
|
||||
rc_2.set_angle(4500);
|
||||
rc_3.set_range(0,1000);
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
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));
|
||||
print_pwm();
|
||||
}
|
||||
|
||||
void print_pwm()
|
||||
{
|
||||
Serial.print("ch1 ");
|
||||
Serial.print(rc_1.control_in, DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(rc_2.control_in, DEC);
|
||||
Serial.print("\tch3 :");
|
||||
Serial.print(rc_3.control_in, DEC);
|
||||
Serial.print("\tch4 :");
|
||||
Serial.print(rc_4.control_in, DEC);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user