mirror of https://github.com/ArduPilot/ardupilot
278 lines
9.1 KiB
C++
278 lines
9.1 KiB
C++
#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
|