You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
277 lines
9.1 KiB
277 lines
9.1 KiB
#ifndef defaultControllers_H |
|
#define defaultControllers_H |
|
|
|
#include "AP_Controller.h" |
|
#include "AP_HardwareAbstractionLayer.h" |
|
#include "../AP_Common/AP_Var.h" |
|
#include <avr/pgmspace.h> |
|
#include "AP_Navigator.h" |
|
|
|
namespace apo { |
|
|
|
class CarController: public AP_Controller { |
|
private: |
|
// control mode |
|
AP_Var_group _group;AP_Uint8 _mode; |
|
enum { |
|
CH_MODE = 0, CH_STR, CH_THR |
|
}; |
|
PidDFB2 pidStr; |
|
Pid2 pidThr; |
|
public: |
|
CarController(AP_Var::Key cntrlKey, AP_Var::Key pidStrKey, |
|
AP_Var::Key pidThrKey, AP_Navigator * nav, AP_Guide * guide, |
|
AP_HardwareAbstractionLayer * hal) : |
|
AP_Controller(nav, guide, hal), _group(cntrlKey, PSTR("CNTRL_")), |
|
_mode(&_group, 1, 0, PSTR("MODE")), |
|
pidStr(pidStrKey, PSTR("STR_"), 1.0, 0, 0, 0, 3), |
|
pidThr(pidThrKey, PSTR("THR_"), 0.6, 0.5, 0, 1, 3) { |
|
_hal->debug->println_P(PSTR("initializing car controller")); |
|
|
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7,1100,1500,1900)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0,1100,1540,1900)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1,1100,1500,1900)); |
|
} |
|
virtual void update(const float & dt) { |
|
// read mode switch |
|
_hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); |
|
// manual |
|
if (_hal->rc[CH_MODE]->getPosition() > 0) { |
|
_hal->rc[CH_STR]->setPwm(_hal->rc[CH_STR]->readRadio()); |
|
_hal->rc[CH_THR]->setPwm(_hal->rc[CH_THR]->readRadio()); |
|
//_hal->debug->println("manual"); |
|
|
|
} else { // auto |
|
float headingError = _guide->headingCommand - _nav->getHeading(); |
|
if (headingError > 180 * deg2Rad) |
|
headingError -= 360 * deg2Rad; |
|
if (headingError < -180 * deg2Rad) |
|
headingError += 360 * deg2Rad; |
|
_hal->rc[CH_STR]->setPosition(pidStr.update(headingError,_nav->getYawRate(),dt)); |
|
_hal->rc[CH_THR]->setPosition(pidThr.update(_guide->groundSpeedCommand - _nav->getGroundSpeed(),dt)); |
|
//_hal->debug->println("automode"); |
|
} |
|
} |
|
}; |
|
|
|
#if CUSTOM_INCLUDES == CUSTOM_MIKROKOPTER |
|
#include "mikrokopter.h" |
|
#endif |
|
|
|
class QuadController: public AP_Controller { |
|
public: |
|
|
|
/** |
|
* note that these are not the controller radio channel numbers, they are just |
|
* unique keys so they can be reaccessed from the hal rc vector |
|
*/ |
|
enum autoChannel_t { |
|
CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order |
|
CH_LEFT, // this enum must match this |
|
CH_RIGHT, |
|
CH_FRONT, |
|
CH_BACK, |
|
CH_ROLL, |
|
CH_PITCH, |
|
CH_YAW, |
|
CH_THRUST |
|
}; |
|
|
|
QuadController(AP_Navigator * nav, AP_Guide * guide, |
|
AP_HardwareAbstractionLayer * hal) : |
|
AP_Controller(nav, guide, hal), |
|
pidRoll(k_pidRoll, PSTR("ROLL_"), PID_ATT_P, PID_ATT_I, |
|
PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), |
|
pidPitch(k_pidPitch, PSTR("PITCH_"), PID_ATT_P, PID_ATT_I, |
|
PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), |
|
pidYaw(k_pidYaw, PSTR("YAW_"), PID_YAWPOS_P, PID_YAWPOS_I, |
|
PID_YAWPOS_D, PID_YAWPOS_AWU, PID_YAWPOS_LIM), |
|
pidYawRate(k_pidYawRate, PSTR("YAWRATE_"), PID_YAWSPEED_P, |
|
PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_AWU, |
|
PID_YAWSPEED_LIM), |
|
pidPN(k_pidPN, PSTR("NORTH_"), PID_POS_P, |
|
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), |
|
pidPE(k_pidPE, PSTR("EAST_"), PID_POS_P, PID_POS_I, |
|
PID_POS_D, PID_POS_AWU, PID_POS_LIM), |
|
pidPD(k_pidPD, PSTR("DOWN_"), PID_POS_Z_P, PID_POS_Z_I, |
|
PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM) |
|
{ |
|
/* |
|
* allocate radio channels |
|
* the order of the channels has to match the enumeration above |
|
*/ |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, 1500, 1900, RC_MODE_IN)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1100, 1900, RC_MODE_OUT)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1100, 1900, RC_MODE_OUT)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, 1100, 1900, RC_MODE_OUT)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_OUT)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, 1500, 1900, RC_MODE_IN)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, 1500, 1900, RC_MODE_IN)); |
|
_hal->rc.push_back( |
|
new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, 1900, RC_MODE_IN)); |
|
_hal->rc.push_back( // -1 -> 0 maps to 1200, linear 0-1 -> 1200-1800 |
|
new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_IN)); |
|
} |
|
|
|
virtual void update(const float & dt) { |
|
|
|
if (commLost()) { |
|
_mode = MAV_MODE_FAILSAFE; |
|
_hal->rc[CH_LEFT]->setPosition(0); |
|
_hal->rc[CH_RIGHT]->setPosition(0); |
|
_hal->rc[CH_FRONT]->setPosition(0); |
|
_hal->rc[CH_BACK]->setPosition(0); |
|
} |
|
|
|
// read and set pwm so they can be read as positions later |
|
_hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); |
|
_hal->rc[CH_ROLL]->setPwm(_hal->rc[CH_ROLL]->readRadio()); |
|
_hal->rc[CH_PITCH]->setPwm(_hal->rc[CH_PITCH]->readRadio()); |
|
_hal->rc[CH_YAW]->setPwm(_hal->rc[CH_YAW]->readRadio()); |
|
_hal->rc[CH_THRUST]->setPwm(_hal->rc[CH_THRUST]->readRadio()); |
|
|
|
// manual mode |
|
float mixRemoteWeight = 0; |
|
if (_hal->rc[CH_MODE]->getPwm() > 1350) mixRemoteWeight = 1; |
|
|
|
// "mix manual" |
|
float cmdRoll = _hal->rc[CH_ROLL]->getPosition() * mixRemoteWeight; |
|
float cmdPitch = _hal->rc[CH_PITCH]->getPosition() * mixRemoteWeight; |
|
float cmdYawRate = _hal->rc[CH_YAW]->getPosition() * mixRemoteWeight; |
|
float thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; |
|
|
|
// position loop |
|
/* |
|
float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt); |
|
float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt); |
|
float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt); |
|
|
|
// "transform-to-body" |
|
{ |
|
float trigSin = sin(-yaw); |
|
float trigCos = cos(-yaw); |
|
_cmdPitch = _cmdEastTilt * trigCos |
|
- _cmdNorthTilt * trigSin; |
|
_cmdRoll = -_cmdEastTilt * trigSin |
|
+ _cmdNorthTilt * trigCos; |
|
// note that the north tilt is negative of the pitch |
|
} |
|
|
|
//thrustMix += THRUST_HOVER_OFFSET; |
|
|
|
// "thrust-trim-adjust" |
|
if (fabs(_cmdRoll) > 0.5) { |
|
_thrustMix *= 1.13949393; |
|
} else { |
|
_thrustMix /= cos(_cmdRoll); |
|
} |
|
if (fabs(_cmdPitch) > 0.5) { |
|
_thrustMix *= 1.13949393; |
|
} else { |
|
_thrustMix /= cos(_cmdPitch); |
|
} |
|
*/ |
|
|
|
// attitude loop |
|
float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),_nav->getRollRate(),dt); |
|
float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),_nav->getPitchRate(),dt); |
|
float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(),dt); |
|
|
|
_hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); |
|
_hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); |
|
_hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); |
|
_hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); |
|
|
|
_hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n", |
|
_hal->rc[CH_LEFT]->getPosition(), |
|
_hal->rc[CH_RIGHT]->getPosition(), |
|
_hal->rc[CH_FRONT]->getPosition(), |
|
_hal->rc[CH_BACK]->getPosition()); |
|
|
|
_hal->debug->printf("rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", |
|
rollMix, |
|
pitchMix, |
|
yawMix, |
|
thrustMix); |
|
|
|
// _hal->debug->printf("thrust pwm: %d\n",_hal->rc[CH_THRUST]->readRadio()); |
|
} |
|
|
|
private: |
|
PidDFB2 pidRoll, pidPitch, pidYaw, pidPN, pidPE, pidPD; |
|
Pid2 pidYawRate; |
|
}; |
|
|
|
/* |
|
class PlaneController : public AP_Controller |
|
{ |
|
private: |
|
// state |
|
AP_Float roll; |
|
AP_Float airspeed; |
|
AP_Float velocity; |
|
AP_Float heading; |
|
|
|
// servo positions |
|
AP_Float steering; |
|
AP_Float throttle; |
|
|
|
// control variables |
|
AP_Float headingCommand; |
|
AP_Float airspeedCommand; |
|
AP_Float rollCommand; |
|
|
|
// channels |
|
static const uint8_t chRoll = 0; |
|
static const uint8_t chPitch = 1; |
|
static const uint8_t chYaw = 2; |
|
|
|
public: |
|
PlaneController(AP_Var::Key chRollKey, AP_Var::Key chPitchKey, AP_Var::Key chYawKey, |
|
AP_Var::Key pidRollKey, AP_Var::Key pidPitchKey, AP_Var::Key pidYawKey) : { |
|
// rc channels |
|
addCh(new AP_RcChannelSimple(chRollKey,PSTR("ROLL"),APM_RC,chRoll,45)); |
|
addCh(new AP_RcChannelSimple(chPitchKey,PSTR("PTCH"),APM_RC,chPitch,45)); |
|
addCh(new AP_RcChannelSimple(chYawKey,PSTR("YAW"),APM_RC,chYaw,45)); |
|
|
|
// pitch control loop |
|
#if AIRSPEED_SENSOR == ENABLED |
|
// pitch control loop w/ airspeed |
|
addBlock(new SumGain(airspeedCommand,AP_Float_unity,airspeed,AP_Float_negative_unity)); |
|
#else |
|
// cross feed variables |
|
addBlock(new SumGain(roll,kffPitchCompk,throttleServo,kffT2P)); |
|
#endif |
|
addBlock(new Pid(pidPitchKey,PSTR("PTCH"),0.1,0,0,1,20)); |
|
addBlock(new ToServo(getRc(chPitch))); |
|
|
|
// roll control loop |
|
addBlock(new SumGain(headingCommand,one,heading,negOne)); |
|
addBlock(new Pid(headingkP,headingKI,headingKD)); |
|
addBlock(new Sink(rollCommand)); |
|
addBlock(new SumGain(rollCommand,one,roll,negOne)); |
|
addBlock(new Pid(rollKP,rollKI,rollKD)); |
|
addBlock(new ToServo(getRc(chRoll))); |
|
|
|
// throttle control loop |
|
addBlock(new SumGain(airspeedCommand,one,airspeed,negOne)); |
|
addBlock(new Pid(throttleKP,throttleKI,throttleKD)); |
|
addBlock(new ToServo(getRc(chThr))); |
|
} |
|
}; |
|
*/ |
|
|
|
} // namespace apo |
|
|
|
#endif // defaultControllers_H |
|
// vim:ts=4:sw=4:expandtab
|
|
|