Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APO Compiling with new AP_Var! 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1612 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-02-08 01:58:51 +00:00
parent af1ee477b9
commit 2584f9a1f1
3 changed files with 27 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_Controller/AP_Controller.h
View File

@ -19,11 +19,15 @@
#ifndef AP_Controller_H #ifndef AP_Controller_H
#define AP_Controller_H #define AP_Controller_H
#include <AP_Common.h>
#include <AP_Vector.h> #include <AP_Vector.h>
#include <AP_Var.h> #include <AP_Var.h>
#include <AP_RcChannel.h> #include <AP_RcChannel.h>
#include <APM_RC.h> #include <APM_RC.h>
#define APVarPtr2Float(var) (*(AP_Meta_class::meta_cast<AP_Float>((AP_Var *)var)))
/// Block /// Block
class Block class Block
{ {
@ -41,7 +45,7 @@ public:
protected: protected:
const char * _name; const char * _name;
Vector< const AP_Var * > _input; Vector< const AP_Var * > _input;
Vector< const AP_Var * > _output; Vector< AP_Var * > _output;
}; };
/// Servo Block /// Servo Block
@ -59,7 +63,7 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_input.getSize() > 0) if (_input.getSize() > 0)
_ch.setPosition(_output[0]); _ch.setPosition(APVarPtr2Float(_output[0]));
} }
private: private:
float _position; float _position;
@ -80,8 +84,7 @@ public:
AP_Var & var5 = AP_Float_zero, AP_Var & gain5 = AP_Float_zero, AP_Var & var5 = AP_Float_zero, AP_Var & gain5 = AP_Float_zero,
AP_Var & var6 = AP_Float_zero, AP_Var & gain6 = AP_Float_zero, AP_Var & var6 = AP_Float_zero, AP_Var & gain6 = AP_Float_zero,
AP_Var & var7 = AP_Float_zero, AP_Var & gain7 = AP_Float_zero, AP_Var & var7 = AP_Float_zero, AP_Var & gain7 = AP_Float_zero,
AP_Var & var8 = AP_Float_zero, AP_Var & gain8 = AP_Float_zero) : AP_Var & var8 = AP_Float_zero, AP_Var & gain8 = AP_Float_zero)
_input(), _gain()
{ {
if ( (&var1 == &AP_Float_zero) || (&gain1 == &AP_Float_zero) ) add(var1,gain1); if ( (&var1 == &AP_Float_zero) || (&gain1 == &AP_Float_zero) ) add(var1,gain1);
if ( (&var2 == &AP_Float_zero) || (&gain2 == &AP_Float_zero) ) add(var2,gain2); if ( (&var2 == &AP_Float_zero) || (&gain2 == &AP_Float_zero) ) add(var2,gain2);
@ -105,10 +108,10 @@ public:
virtual void update(const float & dt = 0) virtual void update(const float & dt = 0)
{ {
if (_output.getSize() < 1) return; if (_output.getSize() < 1) return;
_output[0]->setF(0); _output[0]=0;
for (int i=0;i<_input.getSize();i++) for (int i=0;i<_input.getSize();i++)
{ {
_output[0]->setF( _output[i] + _input[i]*_gain[i]); *_output[0] = APVarPtr2Float(_output[i]) * APVarPtr2Float(_gain[i]) ;
} }
} }
private: private:
@ -124,14 +127,14 @@ public:
float kI = 0.0, float kI = 0.0,
float kD = 0.0, float kD = 0.0,
float iMax = 0.0, float iMax = 0.0,
uint8_t dFcut = 20.0, uint8_t dFcut = 20.0
) : ) :
AP_Var_Group(key,name), AP_Var_group(key,name),
_kP(&_group,0,kP,P_STR("P")), _kP(this,1,kP,PSTR("P")),
_kI(&_group,0,kP,P_STR("I")), _kI(this,2,kI,PSTR("I")),
_kD(&_group,0,kP,P_STR("D")), _kD(this,3,kD,PSTR("D")),
_iMax(&_group,0,kP,P_STR("IMAX")), _iMax(this,4,iMax,PSTR("IMAX")),
_fCut(&_group,0,kP,P_STR("FCUT")), _fCut(this,5,dFcut,PSTR("FCUT"))
{ {
_output.push_back(new AP_Float(0)); _output.push_back(new AP_Float(0));
} }
@ -145,28 +148,28 @@ public:
if (_output.getSize() < 1) return; if (_output.getSize() < 1) return;
// derivative // derivative
float RC = 1/(2*M_PI*_fCut->get()); // low pass filter float RC = 1/(2*M_PI*_fCut); // low pass filter
_eD = _eD + ( ((_e - _input[0]))/dt - _eD ) * (dt / (dt + RC)); _eD = _eD + ( ((_e - APVarPtr2Float(_input[0])))/dt - _eD ) * (dt / (dt + RC));
// proportional, note must come after derivative // proportional, note must come after derivative
// because derivatve uses _e as previous value // because derivatve uses _e as previous value
_e = _input[0]; _e = APVarPtr2Float(_input[0]);
// integral // integral
_eI += _e*dt; _eI += _e*dt;
// pid sum // pid sum
_output[0]->setF(_kP*_e + _kI*_eI + _kD*_eD); *_output[0] = _kP*_e + _kI*_eI + _kD*_eD;
} }
private: private:
float _e; /// input float _e; /// input
float _eI; /// integral of input float _eI; /// integral of input
float _eD; /// derivative of input float _eD; /// derivative of input
AP_Float * _kP; /// proportional gain AP_Float _kP; /// proportional gain
AP_Float * _kI; /// integral gain AP_Float _kI; /// integral gain
AP_Float * _kD; /// derivative gain AP_Float _kD; /// derivative gain
AP_Float * _iMax; /// integrator saturation AP_Float _iMax; /// integrator saturation
AP_Uint8 * _fCut; /// derivative low-pass cut freq (Hz) AP_Uint8 _fCut; /// derivative low-pass cut freq (Hz)
}; };
/// Controller class /// Controller class

2
libraries/AP_RcChannel/AP_RcChannel.cpp
View File

@ -20,7 +20,7 @@ AP_RcChannel::AP_RcChannel(AP_Var::Key & key, const prog_char * name, APM_RC_Cla
const uint16_t & pwmNeutral, const uint16_t & pwmMax, const uint16_t & pwmNeutral, const uint16_t & pwmMax,
const uint16_t & pwmDeadZone, const uint16_t & pwmDeadZone,
const bool & filter, const bool & reverse) : const bool & filter, const bool & reverse) :
AP_Var_scope(name), AP_Var_group(key,name),
_rc(rc), _rc(rc),
ch(this,0,ch,PSTR("CH")), ch(this,0,ch,PSTR("CH")),
scale(this,1,ch,PSTR("SCALE")), scale(this,1,ch,PSTR("SCALE")),

2
libraries/AP_RcChannel/AP_RcChannel.h
View File

@ -8,8 +8,8 @@
#include <stdint.h> #include <stdint.h>
#include <APM_RC.h> #include <APM_RC.h>
#include <AP_Var.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Var.h>
/// @class AP_RcChannel /// @class AP_RcChannel
/// @brief Object managing one RC channel /// @brief Object managing one RC channel