mirror of https://github.com/ArduPilot/ardupilot
Added AP_Variable.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1259 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert
parent
3ce38444b5
commit
60f9a6d216
|
|
@ -83,6 +83,86 @@ struct Location {
|
|||
int32_t lng; ///< param 4 - Longitude * 10**7
|
||||
};
|
||||
|
||||
/**
|
||||
* The parameter template class. This class
|
||||
* implements get/set/save/load etc for the
|
||||
* abstract template type.
|
||||
*/
|
||||
template <class type>
|
||||
class AP_Var
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* The default constrcutor
|
||||
*/
|
||||
AP_Var(type data, const char * name = "", bool sync=false) :
|
||||
_data(data), _name(name), _sync(sync)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the variable value
|
||||
*/
|
||||
void set(type val) {
|
||||
_data = val;
|
||||
if (_sync) save();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the variable value.
|
||||
*/
|
||||
type get() {
|
||||
if (_sync) load();
|
||||
return _data;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the variable value as a float
|
||||
*/
|
||||
void setAsFloat(float val) {
|
||||
set((type)val);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the variable as a float
|
||||
*/
|
||||
float getAsFloat() {
|
||||
return (float)get();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Save a variable to eeprom
|
||||
*/
|
||||
virtual void save()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Load a variable from eeprom
|
||||
*/
|
||||
virtual void load()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the name. This is useful for ground stations.
|
||||
*/
|
||||
const char * getName() { return _name; }
|
||||
|
||||
/**
|
||||
* If sync is true the a load will always occure before a get and a save will always
|
||||
* occure before a set.
|
||||
*/
|
||||
bool getSync() { return _sync; }
|
||||
void setSync(bool sync) { _sync = sync; }
|
||||
|
||||
protected:
|
||||
type _data; /** The data that is stored on the heap */
|
||||
const char * _name; /** The variable name, useful for gcs and terminal output */
|
||||
bool _sync; /** Whether or not to call save/load on get/set */
|
||||
};
|
||||
|
||||
//@}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Vector.h>
|
||||
#include <avr/eeprom.h>
|
||||
#include <avr/pgmspace.h>
|
||||
|
|
@ -29,18 +30,6 @@
|
|||
class AP_EEPromEntry
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Default constructor
|
||||
*/
|
||||
AP_EEPromEntry(const char * name = "") : _id(0), _address(0), _name(name), _sync(false)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Note: The set/get entry functions are virtual and therefore require more machine code, so use set/get in
|
||||
* AP_EEPromVar unless you are accessing via the registry.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Pure virtual function for setting the data value as a float. The function must handle the cast to
|
||||
* the stored variable types.
|
||||
|
|
@ -53,33 +42,20 @@ public:
|
|||
*/
|
||||
virtual float getEntry() = 0;
|
||||
|
||||
/**
|
||||
* Pure virtual function for getting entry name.
|
||||
*/
|
||||
virtual const char * getEntryName() = 0;
|
||||
|
||||
/**
|
||||
* Get the id of the variable.
|
||||
*/
|
||||
uint16_t getId() { return _id; }
|
||||
virtual uint16_t getEntryId() = 0;
|
||||
|
||||
/**
|
||||
* Get the address of the variable.
|
||||
*/
|
||||
uint16_t getAddress() { return _address; }
|
||||
|
||||
/**
|
||||
* Get the name. This is useful for ground stations.
|
||||
*/
|
||||
const char * getName() { return _name; }
|
||||
|
||||
/**
|
||||
* If sync is true the a load will always occure before a get and a save will always
|
||||
* occure before a set.
|
||||
*/
|
||||
bool setSync(bool sync) { _sync = sync; }
|
||||
|
||||
protected:
|
||||
|
||||
uint16_t _id; /** Variable identifier */
|
||||
uint16_t _address; /** EEProm address of variable */
|
||||
const char * _name; /** The name of the variable, a string */
|
||||
bool _sync; /** true: save/load before get/set, false: don't call save/load automatically*/
|
||||
virtual uint16_t getEntryAddress() = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -119,73 +95,35 @@ extern AP_EEPromRegistry eepromRegistry;
|
|||
* abstract template type.
|
||||
*/
|
||||
template <class type>
|
||||
class AP_EEPromVar : public AP_EEPromEntry
|
||||
class AP_EEPromVar : public AP_EEPromEntry, public AP_Var<type>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* The default constrcutor
|
||||
*/
|
||||
AP_EEPromVar(const char * name = "") : AP_EEPromEntry(name)
|
||||
AP_EEPromVar(type data, const char * name = "", bool sync=false) :
|
||||
AP_Var<type>(data,name,sync)
|
||||
{
|
||||
eepromRegistry.add(this,_id,_address,sizeof(_data));
|
||||
eepromRegistry.add(this,_id,_address,sizeof(type));
|
||||
}
|
||||
|
||||
/**
|
||||
* Non-virtual set used for efficient variable setting without
|
||||
* using eepromRegistry
|
||||
*/
|
||||
void set(type val) {
|
||||
_data = val;
|
||||
if (_sync) save();
|
||||
}
|
||||
virtual void setEntry(float val) { this->setAsFloat(val); }
|
||||
virtual float getEntry() { return this->getAsFloat(); }
|
||||
virtual const char * getEntryName() { return this->getName(); }
|
||||
|
||||
/**
|
||||
* Non-virtual get used for efficient getting of a variable
|
||||
* without using eepromRegistry
|
||||
* Get the id of the variable.
|
||||
*/
|
||||
type get() {
|
||||
if (_sync) load();
|
||||
return _data;
|
||||
}
|
||||
virtual uint16_t getEntryId() { return _id; }
|
||||
|
||||
/**
|
||||
* Save a variable to eeprom
|
||||
* Get the address of the variable.
|
||||
*/
|
||||
void save()
|
||||
{
|
||||
eeprom_write_block((const void*)&_data,(void*)(_address),sizeof(_data));
|
||||
}
|
||||
|
||||
/**
|
||||
* Load a variable from eeprom
|
||||
*/
|
||||
void load()
|
||||
{
|
||||
eeprom_read_block((void*)&_data,(const void*)(_address),sizeof(_data));
|
||||
}
|
||||
|
||||
/**
|
||||
* Virtual call that allows an type of variable to be set by a float.
|
||||
* This requires casting and will require some thought if initializing a
|
||||
* boolean etc. Initializing a boolean to false, pass (0.0), true, pass (1.0) etc..
|
||||
*/
|
||||
virtual void setEntry(float val)
|
||||
{
|
||||
_data = (type)val;
|
||||
if (_sync) save();
|
||||
}
|
||||
|
||||
/**
|
||||
* Virtual call that retrieves any variable types as a float.
|
||||
*/
|
||||
virtual float getEntry()
|
||||
{
|
||||
if (_sync) load();
|
||||
return (float)_data;
|
||||
}
|
||||
virtual uint16_t getEntryAddress() { return _address; }
|
||||
|
||||
private:
|
||||
type _data; /** The data that is stored on the heap */
|
||||
uint16_t _id; /** Variable identifier */
|
||||
uint16_t _address; /** EEProm address of variable */
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -5,8 +5,9 @@
|
|||
#include <AP_Common.h>
|
||||
#include <AP_EEProm.h>
|
||||
|
||||
AP_EEPromVar<int> var("TEST_VAR1");
|
||||
AP_EEPromVar<float> var2("TEST_VAR2");
|
||||
AP_EEPromVar<int> var(1,"TEST_VAR1");
|
||||
AP_EEPromVar<float> var2(2.0,"TEST_VAR2");
|
||||
AP_EEPromVar<int16_t> var3(-700,"TEST_VAR3");
|
||||
FastSerialPort0(Serial);
|
||||
|
||||
void setup()
|
||||
|
|
@ -34,8 +35,8 @@ void loop()
|
|||
Serial.printf_P(PSTR("\nval.load(): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
uint16_t id = var.getId();
|
||||
Serial.printf_P(PSTR("\nvar.getId(): %d\n"), id);
|
||||
uint16_t id = var.getEntryId();
|
||||
Serial.printf_P(PSTR("\nvar.getEntryId(): %d\n"), id);
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry[id]->getEntry());
|
||||
|
|
@ -45,8 +46,11 @@ void loop()
|
|||
Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(456): %d\n"), var.get());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getName());
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getAddress()));
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getEntryName());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getEntryAddress()));
|
||||
delay(2000);
|
||||
|
||||
|
||||
Serial.print("\n\nSynced variable demo\n");
|
||||
|
|
@ -65,8 +69,8 @@ void loop()
|
|||
Serial.printf_P(PSTR("\nvar2.load(): %f\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
id = var2.getId();
|
||||
Serial.printf_P(PSTR("\nvar2.getId(): %d\n"), id);
|
||||
id = var2.getEntryId();
|
||||
Serial.printf_P(PSTR("\nvar2.getEntryId(): %d\n"), id);
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\neepromRegistry(id)->getEntry(): %f\n"), eepromRegistry[id]->getEntry());
|
||||
|
|
@ -76,8 +80,10 @@ void loop()
|
|||
Serial.printf_P(PSTR("\neepromRegistry(id)->setEntry(4.56): %f\n"), var2.get());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getName());
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getAddress()));
|
||||
Serial.printf_P(PSTR("\nprint the parameters name by id: %s\n"), eepromRegistry[id]->getEntryName());
|
||||
delay(2000);
|
||||
|
||||
Serial.printf_P(PSTR("\nprint the parameters address by id: %d\n"), int(eepromRegistry[id]->getEntryAddress()));
|
||||
|
||||
delay(5000);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/*
|
||||
RC_ChannelB.cpp - Radio library for Arduino
|
||||
Code by Jason Short. DIYDrones.com
|
||||
Code by Jason Short, James Goppert. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and / or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
|
|
@ -14,180 +14,97 @@
|
|||
#include "WProgram.h"
|
||||
#include "RC_ChannelB.h"
|
||||
|
||||
#define ANGLE 0
|
||||
#define RANGE 1
|
||||
|
||||
// setup the control preferences
|
||||
void
|
||||
RC_ChannelB::set_range(int low, int high)
|
||||
void
|
||||
RC_ChannelB::readRadio(uint16_t pwmRadio)
|
||||
{
|
||||
_type = RANGE;
|
||||
_high = high;
|
||||
_low = low;
|
||||
// apply reverse
|
||||
if(_reverse) _pwmRadio = (_pwmNeutral - pwmRadio) + _pwmNeutral;
|
||||
else _pwmRadio = pwmRadio;
|
||||
|
||||
setPwm(pwmRadio);
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::set_angle(int angle)
|
||||
RC_ChannelB::setPwm(uint16_t pwm)
|
||||
{
|
||||
_type = ANGLE;
|
||||
_high = angle;
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::set_reverse(bool reverse)
|
||||
{
|
||||
if (reverse) _reverse = -1;
|
||||
else _reverse = 1;
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::set_filter(bool filter)
|
||||
{
|
||||
_filter = filter;
|
||||
}
|
||||
|
||||
// call after first read
|
||||
void
|
||||
RC_ChannelB::trim()
|
||||
{
|
||||
radio_trim = radio_in;
|
||||
|
||||
}
|
||||
|
||||
// read input from APM_RC - create a control_in value
|
||||
void
|
||||
RC_ChannelB::set_pwm(int pwm)
|
||||
{
|
||||
//Serial.print(pwm,DEC);
|
||||
// apply reverse
|
||||
if(_reverse) pwm = (_pwmNeutral-pwm) + _pwmNeutral;
|
||||
|
||||
// apply filter
|
||||
if(_filter){
|
||||
if(radio_in == 0)
|
||||
radio_in = pwm;
|
||||
if(_pwm == 0)
|
||||
_pwm = pwm;
|
||||
else
|
||||
radio_in = ((pwm + radio_in) >> 1); // Small filtering
|
||||
_pwm = ((pwm + _pwm) >> 1); // Small filtering
|
||||
}else{
|
||||
radio_in = pwm;
|
||||
_pwm = pwm;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
// apply deadzone
|
||||
_pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm;
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::setPosition(float position)
|
||||
{
|
||||
setPwm(_positionToPwm(position));
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::mixRadio(uint16_t infStart)
|
||||
{
|
||||
float inf = abs(_pwmRadio - _pwmNeutral);
|
||||
inf = min(inf, infStart);
|
||||
inf = ((infStart - inf) /infStart);
|
||||
setPwm(_pwm*inf + _pwmRadio);
|
||||
}
|
||||
|
||||
uint16_t
|
||||
RC_ChannelB::_positionToPwm(float position)
|
||||
{
|
||||
if(position < 0)
|
||||
return (position / _scale) * (_pwmMin - _pwmNeutral);
|
||||
else
|
||||
return (position / _scale) * (_pwmMax - _pwmNeutral);
|
||||
}
|
||||
|
||||
float
|
||||
RC_ChannelB::_pwmToPosition(uint16_t pwm)
|
||||
{
|
||||
if(_pwm < _pwmNeutral)
|
||||
return _scale * (_pwm - _pwmNeutral)/(_pwmNeutral - _pwmMin);
|
||||
else
|
||||
return _scale * (_pwm - _pwmNeutral)/(_pwmMax - _pwmNeutral);
|
||||
}
|
||||
|
||||
void
|
||||
RC_ChannelB::loadEEProm()
|
||||
{
|
||||
if (_storageType == STORE_EEPROM)
|
||||
{
|
||||
eeprom_read_block((void*)&_scale,(const void*)(_address),sizeof(_scale));
|
||||
eeprom_read_block((void*)&_pwmMin,(const void*)(_address + 4),sizeof(_pwmMin));
|
||||
eeprom_read_block((void*)&_pwmMax,(const void*)(_address + 6),sizeof(_pwmMax));
|
||||
eeprom_read_block((void*)&_pwmNeutral,(const void*)(_address + 8),sizeof(_pwmNeutral));
|
||||
eeprom_read_block((void*)&_pwmDeadZone,(const void*)(_address + 10),sizeof(_pwmDeadZone));
|
||||
eeprom_read_block((void*)&_filter,(const void*)(_address+12),sizeof(_filter));
|
||||
eeprom_read_block((void*)&_pwmDeadZone,(const void*)(_address+13),sizeof(_pwmDeadZone));
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
RC_ChannelB::control_mix(float value)
|
||||
{
|
||||
return (1 - abs(control_in / _high)) * value + control_in;
|
||||
}
|
||||
|
||||
// are we below a threshold?
|
||||
bool
|
||||
RC_ChannelB::get_failsafe(void)
|
||||
{
|
||||
return (radio_in < (radio_min - 50));
|
||||
}
|
||||
|
||||
// returns just the PWM without the offset from radio_min
|
||||
void
|
||||
RC_ChannelB::calc_pwm(void)
|
||||
RC_ChannelB::saveEEProm()
|
||||
{
|
||||
|
||||
if(_type == RANGE){
|
||||
pwm_out = range_to_pwm();
|
||||
}else{
|
||||
pwm_out = angle_to_pwm();
|
||||
if (_storageType == STORE_EEPROM)
|
||||
{
|
||||
eeprom_write_block((const void*)&_scale,(void*)(_address),sizeof(_scale));
|
||||
eeprom_write_block((const void*)&_pwmMin,(void*)(_address + 4),sizeof(_pwmMin));
|
||||
eeprom_write_block((const void*)&_pwmMax,(void*)(_address + 6),sizeof(_pwmMax));
|
||||
eeprom_write_block((const void*)&_pwmNeutral,(void*)(_address + 8),sizeof(_pwmNeutral));
|
||||
eeprom_write_block((const void*)&_pwmDeadZone,(void*)(_address + 10),sizeof(_pwmDeadZone));
|
||||
eeprom_write_block((const void*)&_filter,(void*)(_address+12),sizeof(_filter));
|
||||
eeprom_write_block((const void*)&_pwmDeadZone,(void*)(_address+13),sizeof(_pwmDeadZone));
|
||||
}
|
||||
radio_out = pwm_out + radio_min;
|
||||
}
|
||||
|
||||
// ------------------------------------------
|
||||
|
||||
void
|
||||
RC_ChannelB::load_eeprom(void)
|
||||
{
|
||||
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_ChannelB::save_eeprom(void)
|
||||
{
|
||||
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_ChannelB::save_trim(void)
|
||||
{
|
||||
eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
|
||||
}
|
||||
|
||||
// ------------------------------------------
|
||||
|
||||
void
|
||||
RC_ChannelB::update_min_max()
|
||||
{
|
||||
radio_min = min(radio_min, radio_in);
|
||||
radio_max = max(radio_max, radio_in);
|
||||
}
|
||||
|
||||
// ------------------------------------------
|
||||
|
||||
int16_t
|
||||
RC_ChannelB::pwm_to_angle()
|
||||
{
|
||||
if(radio_in < radio_trim)
|
||||
return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
|
||||
else
|
||||
return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim));
|
||||
}
|
||||
|
||||
float
|
||||
RC_ChannelB::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_ChannelB::norm_output()
|
||||
{
|
||||
if(radio_out < radio_trim)
|
||||
return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min);
|
||||
else
|
||||
return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim);
|
||||
}
|
||||
|
||||
int16_t
|
||||
RC_ChannelB::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_ChannelB::pwm_to_range()
|
||||
{
|
||||
return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
|
||||
}
|
||||
|
||||
int16_t
|
||||
RC_ChannelB::range_to_pwm()
|
||||
{
|
||||
return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -7,95 +7,128 @@
|
|||
#define RC_ChannelB_h
|
||||
|
||||
#include <stdint.h>
|
||||
#include <AP_EEProm.h>
|
||||
|
||||
/// @class RC_ChannelB
|
||||
/// @brief Object managing one RC channel
|
||||
class RC_ChannelB{
|
||||
public:
|
||||
|
||||
public:
|
||||
|
||||
enum storage_t
|
||||
{
|
||||
STORE_EXTERNAL,
|
||||
STORE_EEPROM
|
||||
};
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// A RC_ChannelB constructed in this fashion does not support save/restore.
|
||||
///
|
||||
RC_ChannelB() :
|
||||
_address(0)
|
||||
{}
|
||||
_scale(0),
|
||||
_pwmMin(1000),
|
||||
_pwmMax(2000),
|
||||
_pwmNeutral(1500),
|
||||
_pwmDeadZone(100),
|
||||
_pwm(0),
|
||||
_pwmRadio(0),
|
||||
_filter(true),
|
||||
_reverse(false),
|
||||
_address(0),
|
||||
_storageType(STORE_EXTERNAL)
|
||||
{
|
||||
}
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
/// @param address EEPROM base address at which RC_ChannelB parameters
|
||||
/// are stored. Zero if the RC_ChannelB does not support
|
||||
/// save/restore.
|
||||
RC_ChannelB() :
|
||||
_scale(0),
|
||||
_pwmMin(1000),
|
||||
_pwmNeutral(1500),
|
||||
_pwmMax(2000),
|
||||
_pwmDeadZone(100),
|
||||
_pwm(0),
|
||||
_pwmRadio(0),
|
||||
_filter(true),
|
||||
_reverse(false),
|
||||
_address(address),
|
||||
_storageType(STORE_EEPROM)
|
||||
{
|
||||
}
|
||||
|
||||
/// Constructor
|
||||
///
|
||||
RC_ChannelB(uint16_t address) :
|
||||
_scale(0),
|
||||
_pwmMin(1000),
|
||||
_pwmNeutral(1500),
|
||||
_pwmMax(2000),
|
||||
_pwmDeadZone(100),
|
||||
_pwm(0),
|
||||
_pwmRadio(0),
|
||||
_filter(true),
|
||||
_reverse(false),
|
||||
_address(address),
|
||||
_high(1),
|
||||
_filter(true)
|
||||
{}
|
||||
_storageType(STORE_EEPROM)
|
||||
{
|
||||
}
|
||||
|
||||
// setup min and max radio values in CLI
|
||||
void update_min_max();
|
||||
|
||||
// startup
|
||||
void load_eeprom(void);
|
||||
void save_eeprom(void);
|
||||
void save_trim(void);
|
||||
void set_filter(bool filter);
|
||||
// set configuration
|
||||
void setScale(float scale) { _scale = scale; }
|
||||
void setReverse(bool reverse) { _reverse = reverse; }
|
||||
void setFilter(bool filter) { _filter = filter; }
|
||||
void setPwmMin(uint16_t pwmMin) { _pwmMin = pwmMin; }
|
||||
void setPwmMax(uint16_t pwmMax) { _pwmMax = pwmMax; }
|
||||
void setPwmNeutral(uint16_t pwmNeutral) { _pwmNeutral = pwmNeutral; }
|
||||
void setPwmNeutral() { _pwmNeutral = getPwm(); }
|
||||
void setPwmDeadZone(uint16_t pwmDeadZone) { _pwmDeadZone = pwmDeadZone; }
|
||||
|
||||
// setup the control preferences
|
||||
void set_range(int low, int high);
|
||||
void set_angle(int angle);
|
||||
void set_reverse(bool reverse);
|
||||
// save/load
|
||||
void saveEEProm();
|
||||
void loadEEProm();
|
||||
|
||||
// read input from APM_RC - create a control_in value
|
||||
void set_pwm(int pwm);
|
||||
|
||||
// pwm is stored here
|
||||
int16_t radio_in;
|
||||
// get configuration
|
||||
float getScale() { return _scale; }
|
||||
bool getReverse() { return _reverse; }
|
||||
bool getFilter() { return _filter; }
|
||||
uint16_t getPwmMin() { return _pwmMin; }
|
||||
uint16_t getPwmMax() { return _pwmMax; }
|
||||
uint16_t getPwmNeutral() { return _pwmNeutral; }
|
||||
uint16_t getPwmDeadZone() { return _pwmDeadZone; }
|
||||
|
||||
// call after first set_pwm
|
||||
void trim();
|
||||
// set servo state
|
||||
void readRadio(uint16_t pwmRadio);
|
||||
void setPwm(uint16_t pwm);
|
||||
void setPosition(float position);
|
||||
void mixRadio(uint16_t infStart);
|
||||
|
||||
// get servo state
|
||||
uint16_t getPwm() { return _pwm; }
|
||||
uint16_t getPwmRadio() { return _pwmRadio; }
|
||||
float getPosition() { return _pwmToPosition(_pwm); }
|
||||
float getNormalized() { return getPosition()/_scale; }
|
||||
|
||||
// did our read come in 50µs below the min?
|
||||
bool get_failsafe(void);
|
||||
|
||||
// value generated from PWM
|
||||
int16_t control_in;
|
||||
int16_t dead_zone; // used to keep noise down and create a dead zone.
|
||||
|
||||
int control_mix(float value);
|
||||
|
||||
// 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;
|
||||
bool failSafe() { _pwm < (_pwmMin - 50); }
|
||||
|
||||
// generate PWM from servo_out value
|
||||
void calc_pwm(void);
|
||||
private:
|
||||
|
||||
// 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;
|
||||
float _scale;
|
||||
AP_EEProm<uint16_t> _pwmMin;
|
||||
uint16_t _pwmNeutral;
|
||||
uint16_t _pwmMax;
|
||||
uint16_t _pwmDeadZone;
|
||||
uint16_t _pwm; // this is the internal state, positino is just created when needed
|
||||
uint16_t _pwmRadio; // radio pwm input
|
||||
|
||||
// includes offset from PWM
|
||||
//int16_t get_radio_out(void);
|
||||
// configuration
|
||||
bool _filter;
|
||||
int8_t _reverse;
|
||||
storage_t _storageType;
|
||||
uint16_t _address;
|
||||
|
||||
int16_t pwm_to_angle();
|
||||
float norm_input();
|
||||
float norm_output();
|
||||
int16_t angle_to_pwm();
|
||||
int16_t pwm_to_range();
|
||||
int16_t range_to_pwm();
|
||||
|
||||
private:
|
||||
bool _filter;
|
||||
int8_t _reverse;
|
||||
|
||||
int16_t _address; ///< EEPROM address for save/restore of P/I/D
|
||||
bool _type;
|
||||
int16_t _high;
|
||||
int16_t _low;
|
||||
// private methods
|
||||
uint16_t _positionToPwm(float position);
|
||||
float _pwmToPosition(uint16_t pwm);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,170 +6,28 @@
|
|||
*/
|
||||
|
||||
#include <APM_RC.h> // ArduPilot Mega RC Library
|
||||
#include <RC_Channel.h> // ArduPilot Mega RC Library
|
||||
#include <RC_ChannelB.h> // ArduPilot Mega RC Library
|
||||
#include <AP_Common.h>
|
||||
#include <AP_EEProm.h>
|
||||
|
||||
|
||||
#define EE_RADIO_1 0x00 // all gains stored from here
|
||||
#define EE_RADIO_2 0x06 // all gains stored from here
|
||||
#define EE_RADIO_3 0x0C // all gains stored from here
|
||||
#define EE_RADIO_4 0x12 // all gains stored from here
|
||||
#define EE_RADIO_5 0x18 // all gains stored from here
|
||||
#define EE_RADIO_6 0x1E // all gains stored from here
|
||||
#define EE_RADIO_7 0x24 // all gains stored from here
|
||||
#define EE_RADIO_8 0x2A // all gains stored from here
|
||||
|
||||
|
||||
RC_Channel rc_1(EE_RADIO_1);
|
||||
RC_Channel rc_2(EE_RADIO_2);
|
||||
RC_Channel rc_3(EE_RADIO_3);
|
||||
RC_Channel rc_4(EE_RADIO_4);
|
||||
RC_Channel rc_5(EE_RADIO_5);
|
||||
RC_Channel rc_6(EE_RADIO_6);
|
||||
RC_Channel rc_7(EE_RADIO_7);
|
||||
RC_Channel rc_8(EE_RADIO_8);
|
||||
|
||||
#define CH_1 0
|
||||
#define CH_2 1
|
||||
#define CH_3 2
|
||||
#define CH_4 3
|
||||
#define CH_5 4
|
||||
#define CH_6 5
|
||||
#define CH_7 6
|
||||
#define CH_8 7
|
||||
RC_ChannelB rc[] =
|
||||
{
|
||||
RC_ChannelB()
|
||||
};
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.begin(115200);
|
||||
Serial.println("ArduPilot RC Channel test");
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
|
||||
delay(1000);
|
||||
// setup radio
|
||||
|
||||
// read eepom or set manually
|
||||
/*
|
||||
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;
|
||||
*/
|
||||
|
||||
|
||||
rc_1.load_eeprom();
|
||||
rc_2.load_eeprom();
|
||||
rc_3.load_eeprom();
|
||||
rc_4.load_eeprom();
|
||||
rc_5.load_eeprom();
|
||||
rc_6.load_eeprom();
|
||||
rc_7.load_eeprom();
|
||||
rc_8.load_eeprom();
|
||||
|
||||
print_radio_values();
|
||||
|
||||
// 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);
|
||||
rc_5.set_range(0,1000);
|
||||
rc_6.set_range(0,1000);
|
||||
rc_7.set_range(0,1000);
|
||||
rc_8.set_range(0,1000);
|
||||
|
||||
// set midpoint value
|
||||
rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
||||
rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
||||
rc_4.set_pwm(APM_RC.InputCh(CH_4));
|
||||
|
||||
rc_1.trim();
|
||||
rc_2.trim();
|
||||
rc_4.trim();
|
||||
|
||||
|
||||
}
|
||||
|
||||
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));
|
||||
rc_5.set_pwm(APM_RC.InputCh(CH_5));
|
||||
rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
||||
rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
||||
rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
||||
|
||||
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);
|
||||
Serial.print("\tch5 :");
|
||||
Serial.print(rc_5.control_in, DEC);
|
||||
Serial.print("\tch6 :");
|
||||
Serial.print(rc_6.control_in, DEC);
|
||||
Serial.print("\tch7 :");
|
||||
Serial.print(rc_7.control_in, DEC);
|
||||
Serial.print("\tch8 :");
|
||||
Serial.println(rc_8.control_in, DEC);
|
||||
}
|
||||
|
||||
void
|
||||
print_radio_values()
|
||||
{
|
||||
Serial.print("CH1: ");
|
||||
Serial.print(rc_1.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_1.radio_max, DEC);
|
||||
|
||||
Serial.print("CH2: ");
|
||||
Serial.print(rc_2.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_2.radio_max, DEC);
|
||||
|
||||
Serial.print("CH3: ");
|
||||
Serial.print(rc_3.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_3.radio_max, DEC);
|
||||
|
||||
Serial.print("CH4: ");
|
||||
Serial.print(rc_4.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_4.radio_max, DEC);
|
||||
|
||||
Serial.print("CH5: ");
|
||||
Serial.print(rc_5.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_5.radio_max, DEC);
|
||||
|
||||
Serial.print("CH6: ");
|
||||
Serial.print(rc_6.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_6.radio_max, DEC);
|
||||
|
||||
Serial.print("CH7: ");
|
||||
Serial.print(rc_7.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_7.radio_max, DEC);
|
||||
|
||||
Serial.print("CH8: ");
|
||||
Serial.print(rc_8.radio_min, DEC);
|
||||
Serial.print(" | ");
|
||||
Serial.println(rc_8.radio_max, DEC);
|
||||
Serial.println("looping");
|
||||
delay(1000);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue