Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Updated APO Controllers.

Browse Source
This commit is contained in:
James Goppert 2011-10-25 19:40:07 -04:00
parent 97ef98fa8f
commit dd4ff8c0aa
12 changed files with 381 additions and 476 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduBoat/BoatGeneric.h
View File

@ -3,6 +3,7 @@
*
* Created on: May 1, 2011
* Author: jgoppert
*
*/
#ifndef BOATGENERIC_H_
@ -61,19 +62,24 @@ static const float loop2Rate = 1; // gcs slow
static const float loop3Rate = 0.1;
// gains
const float steeringP = 1.0;
const float steeringI = 0.0;
const float steeringD = 0.0;
const float steeringIMax = 0.0;
const float steeringYMax = 3.0;
const float steeringDFCut = 25;
static const float steeringP = 0.1;
static const float steeringI = 0.0;
static const float steeringD = 0.1;
static const float steeringIMax = 0.0;
static const float steeringYMax = 1;
static const float steeringDFCut = 25.0;
const float throttleP = 0.0;
const float throttleI = 0.0;
const float throttleD = 0.0;
const float throttleIMax = 0.0;
const float throttleYMax = 0.0;
const float throttleDFCut = 3.0;
static const float throttleP = 0.1;
static const float throttleI = 0.0;
static const float throttleD = 0.0;
static const float throttleIMax = 0.0;
static const float throttleYMax = 1;
static const float throttleDFCut = 0.0;
// guidance
static const float velCmd = 5;
static const float xt = 10;
static const float xtLim = 90;
#include "ControllerBoat.h"

153
ArduBoat/ControllerBoat.h
View File

@ -13,111 +13,98 @@
namespace apo {
class ControllerBoat: public AP_Controller {
private:
AP_Var_group _group;
AP_Uint8 _mode;
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThr
};
enum {
k_pidStr = k_controllersStart, k_pidThr
};
enum {
CH_MODE = 0, CH_STR, CH_THR
};
BlockPIDDfb pidStr;
BlockPID pidThr;
public:
enum {
ch_mode = 0, ch_str, ch_thrust
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
};
ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal),
_group(k_cntrl, PSTR("CNTRL_")),
_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut) {
throttleDFCut), _strCmd(0), _thrustCmd(0)
{
_hal->debug->println_P(PSTR("initializing boat controller"));
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100,
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540,
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 3, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get();
private:
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
}
virtual void update(const float & dt) {
// check for heartbeat
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
return;
// if throttle less than 5% cut motor power
} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
_mode = MAV_MODE_LOCKED;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_STANDBY);
return;
// if in live mode then set state to active
} else if (_hal->getMode() == MODE_LIVE) {
_hal->setState(MAV_STATE_ACTIVE);
// if in hardware in the loop (control) mode, set to hilsim
} else if (_hal->getMode() == MODE_HIL_CNTL) {
_hal->setState(MAV_STATE_HILSIM);
}
void autoLoop(const float dt) {
_hal->debug->printf_P(PSTR("hdg cmd: %f, yaw: %f\n"),_guide->getHeadingCommand(),_nav->getYaw());
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_strCmd = pidStr.update(headingError, _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
// read switch to set mode
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
_mode = MAV_MODE_MANUAL;
} else {
_mode = MAV_MODE_AUTO;
}
void setMotors() {
// manual mode
switch (_mode) {
switch (_hal->getState()) {
case MAV_MODE_MANUAL: {
setAllRadioChannelsManually();
//_hal->debug->println("manual");
break;
}
case MAV_MODE_AUTO: {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
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->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt));
//_hal->debug->println("automode");
break;
}
default: {
setAllRadioChannelsToNeutral();
_mode = MAV_MODE_FAILSAFE;
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("unknown controller mode\n"));
break;
}
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_thrust]->setPosition(_thrustCmd);
_hal->rc[ch_str]->setPosition(_strCmd);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
}
}
};
} // namespace apo

1
ArduRover/ArduRover.pde
View File

@ -18,6 +18,7 @@
// Vehicle Configuration
#include "CarStampede.h"
//#include "TankGeneric.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"

19
ArduRover/CarStampede.h
View File

@ -62,19 +62,24 @@ static const float loop2Rate = 1; // gcs slow
static const float loop3Rate = 0.1;
// gains
static const float steeringP = 1.0;
static const float steeringP = 0.1;
static const float steeringI = 0.0;
static const float steeringD = 0.0;
static const float steeringD = 0.1;
static const float steeringIMax = 0.0;
static const float steeringYMax = 3.0;
static const float steeringDFCut = 25;
static const float steeringYMax = 1;
static const float steeringDFCut = 25.0;
static const float throttleP = 0.0;
static const float throttleP = 0.1;
static const float throttleI = 0.0;
static const float throttleD = 0.0;
static const float throttleIMax = 0.0;
static const float throttleYMax = 0.0;
static const float throttleDFCut = 3.0;
static const float throttleYMax = 1;
static const float throttleDFCut = 0.0;
// guidance
static const float velCmd = 5;
static const float xt = 10;
static const float xtLim = 90;
#include "ControllerCar.h"

152
ArduRover/ControllerCar.h
View File

@ -13,111 +13,99 @@
namespace apo {
class ControllerCar: public AP_Controller {
private:
AP_Var_group _group;
AP_Uint8 _mode;
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThr
};
enum {
k_pidStr = k_controllersStart, k_pidThr
};
enum {
CH_MODE = 0, CH_STR, CH_THR
};
BlockPIDDfb pidStr;
BlockPID pidThr;
public:
enum {
ch_mode = 0, ch_str, ch_thrust
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
};
ControllerCar(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal),
_group(k_cntrl, PSTR("CNTRL_")),
_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut) {
throttleDFCut), _strCmd(0), _thrustCmd(0)
{
_hal->debug->println_P(PSTR("initializing car controller"));
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100,
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540,
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 3, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get();
private:
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
}
virtual void update(const float & dt) {
// check for heartbeat
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
return;
// if throttle less than 5% cut motor power
} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
_mode = MAV_MODE_LOCKED;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_STANDBY);
return;
// if in live mode then set state to active
} else if (_hal->getMode() == MODE_LIVE) {
_hal->setState(MAV_STATE_ACTIVE);
// if in hardware in the loop (control) mode, set to hilsim
} else if (_hal->getMode() == MODE_HIL_CNTL) {
_hal->setState(MAV_STATE_HILSIM);
}
// read switch to set mode
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
_mode = MAV_MODE_MANUAL;
} else {
_mode = MAV_MODE_AUTO;
}
void autoLoop(const float dt) {
_hal->debug->printf_P(PSTR("hdg cmd: %f, yaw: %f\n"),_guide->getHeadingCommand(),_nav->getYaw());
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_strCmd = pidStr.update(headingError, _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
// manual mode
switch (_mode) {
void setMotors() {
case MAV_MODE_MANUAL: {
setAllRadioChannelsManually();
//_hal->debug->println("manual");
break;
}
case MAV_MODE_AUTO: {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
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->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt));
//_hal->debug->println("automode");
break;
}
switch (_hal->getState()) {
default: {
setAllRadioChannelsToNeutral();
_mode = MAV_MODE_FAILSAFE;
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("unknown controller mode\n"));
break;
}
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_thrust]->setPosition(_thrustCmd);
_hal->rc[ch_str]->setPosition(_strCmd);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
}
}
};
} // namespace apo

137
ArduRover/ControllerTank.h
View File

@ -13,9 +13,7 @@
namespace apo {
class ControllerTank: public AP_Controller {
private:
AP_Var_group _group;
AP_Uint8 _mode;
public:
enum {
k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr
};
@ -23,21 +21,17 @@ private:
k_pidStr = k_controllersStart, k_pidThr
};
enum {
CH_MODE = 0, CH_LEFT, CH_RIGHT, CH_STR, CH_THR
ch_mode = 0, ch_left, ch_right, ch_str, ch_thrust
};
BlockPIDDfb pidStr;
BlockPID pidThr;
public:
ControllerTank(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal),
_group(k_cntrl, PSTR("CNTRL_")),
_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut) {
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _headingOutput(0), _throttleOutput(0) {
_hal->debug->println_P(PSTR("initializing tank controller"));
_hal->rc.push_back(
@ -56,76 +50,63 @@ public:
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
1900, RC_MODE_IN, false));
}
virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get();
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_headingOutput = _hal->rc[ch_str]->getPosition();
_throttleOutput = _hal->rc[ch_thrust]->getPosition();
}
void mix(float headingOutput, float throttleOutput) {
_hal->rc[CH_LEFT]->setPosition(throttleOutput + headingOutput);
_hal->rc[CH_RIGHT]->setPosition(throttleOutput - headingOutput);
void autoLoop(const float dt) {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_headingOutput = pidStr.update(headingError, _nav->getYawRate(), dt);
_throttleOutput = pidThr.update(_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
virtual void update(const float & dt) {
// check for heartbeat
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
return;
// if throttle less than 5% cut motor power
} else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) {
_mode = MAV_MODE_LOCKED;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_STANDBY);
return;
// if in live mode then set state to active
} else if (_hal->getMode() == MODE_LIVE) {
_hal->setState(MAV_STATE_ACTIVE);
// if in hardware in the loop (control) mode, set to hilsim
} else if (_hal->getMode() == MODE_HIL_CNTL) {
_hal->setState(MAV_STATE_HILSIM);
}
// read switch to set mode
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
_mode = MAV_MODE_MANUAL;
} else {
_mode = MAV_MODE_AUTO;
}
void setMotors() {
// manual mode
switch (_mode) {
switch (_hal->getState()) {
case MAV_MODE_MANUAL: {
setAllRadioChannelsManually();
mix(_hal->rc[CH_STR]->getPosition(),
_hal->rc[CH_THR]->getPosition());
break;
}
case MAV_MODE_AUTO: {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
mix(pidStr.update(headingError, _nav->getYawRate(), dt),
pidThr.update(_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt));
//_hal->debug->println("automode");
break;
}
default: {
setAllRadioChannelsToNeutral();
_mode = MAV_MODE_FAILSAFE;
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("unknown controller mode\n"));
break;
}
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_left]->setPosition(_throttleOutput + _headingOutput);
_hal->rc[ch_right]->setPosition(_throttleOutput - _headingOutput);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
}
}
private:
BlockPIDDfb pidStr;
BlockPID pidThr;
float _headingOutput;
float _throttleOutput;
};
} // namespace apo

7
ArduRover/TankGeneric.h
View File

@ -5,6 +5,8 @@
* Author: jgoppert
*/
// NOT CURRENTLY WORKING
#ifndef TANKGENERIC_H_
#define TANKGENERIC_H_
@ -75,6 +77,11 @@ const float throttleIMax = 0.0;
const float throttleYMax = 0.0;
const float throttleDFCut = 3.0;
// guidance
static const float velCmd = 5;
static const float xt = 10;
static const float xtLim = 90;
#include "ControllerTank.h"
#endif /* TANKGENERIC_H_ */

214
apo/ControllerPlane.h
View File

@ -13,18 +13,9 @@
namespace apo {
class ControllerPlane: public AP_Controller {
private:
AP_Var_group _group;
AP_Var_group _trimGroup;
AP_Uint8 _mode;
AP_Uint8 _rdrAilMix;
bool _needsTrim;
AP_Float _ailTrim;
AP_Float _elvTrim;
AP_Float _rdrTrim;
AP_Float _thrTrim;
public:
enum {
ch_mode = 0, ch_roll, ch_pitch, ch_thr, ch_yaw
ch_mode = 0, ch_roll, ch_pitch, ch_thrust, ch_yaw
};
enum {
k_chMode = k_radioChannelsStart,
@ -42,21 +33,11 @@ private:
k_trim = k_customStart
};
BlockPID pidBnkRll; // bank error to roll servo deflection
BlockPID pidSpdPit; // speed error to pitch command
BlockPID pidPitPit; // pitch error to pitch servo deflection
BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection
BlockPID pidHdgBnk; // heading error to bank command
BlockPID pidAltThr; // altitude error to throttle deflection
bool requireRadio;
public:
ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal),
_group(k_cntrl, PSTR("cntrl_")),
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
_trimGroup(k_trim, PSTR("trim_")),
_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("mode")),
_rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
_needsTrim(false),
_ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")),
@ -81,7 +62,7 @@ public:
pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1,
pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu,
pidAltThrLim, pidAltThrDFCut),
requireRadio(false) {
requireRadio(false), _aileron(0), _elevator(0), _rudder(0), _throttle(0) {
_hal->debug->println_P(PSTR("initializing plane controller"));
@ -101,113 +82,112 @@ public:
new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500,
1800, RC_MODE_INOUT, false));
}
virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get();
void manualLoop(const float dt) {
setAllRadioChannelsManually();
// force auto to read new manual trim
if (_needsTrim == false)
_needsTrim = true;
//_hal->debug->println("manual");
}
virtual void update(const float & dt) {
// check for heartbeat
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
return;
// if the value of the throttle is less than 5% cut motor power
} else if (requireRadio && _hal->rc[ch_thr]->getRadioPosition() < 0.05) {
_mode = MAV_MODE_LOCKED;
setAllRadioChannelsToNeutral();
_hal->setState(MAV_STATE_STANDBY);
return;
// if in live mode then set state to active
} else if (_hal->getMode() == MODE_LIVE) {
_hal->setState(MAV_STATE_ACTIVE);
// if in hardware in the loop (control) mode, set to hilsim
} else if (_hal->getMode() == MODE_HIL_CNTL) {
_hal->setState(MAV_STATE_HILSIM);
void autoLoop(const float dt) {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_aileron = pidBnkRll.update(
pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt);
_elevator = pidPitPit.update(
-pidSpdPit.update(
_guide->getAirSpeedCommand() - _nav->getAirSpeed(),
dt) - _nav->getPitch(), dt);
_rudder = pidYwrYaw.update(-_nav->getYawRate(), dt);
// desired yaw rate is zero, needs washout
_throttle = pidAltThr.update(
_guide->getAltitudeCommand() - _nav->getAlt(), dt);
// if needs trim
if (_needsTrim) {
// need to subtract current controller deflections so control
// surfaces are actually at the same position as manual flight
_ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron;
_elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator;
_rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder;
_thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle;
_needsTrim = false;
}
// read switch to set mode
if (_hal->rc[ch_mode]->getRadioPosition() > 0) {
_mode = MAV_MODE_MANUAL;
} else {
_mode = MAV_MODE_AUTO;
}
// actuator mixing/ output
_aileron += _rdrAilMix * _rudder + _ailTrim;
_elevator += _elvTrim;
_rudder += _rdrTrim;
_throttle += _thrTrim;
// manual mode
switch (_mode) {
//_hal->debug->println("automode");
}
case MAV_MODE_MANUAL: {
setAllRadioChannelsManually();
void setMotors() {
// force auto to read new manual trim
if (_needsTrim == false)
_needsTrim = true;
//_hal->debug->println("manual");
break;
}
case MAV_MODE_AUTO: {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
switch (_hal->getState()) {
float aileron = pidBnkRll.update(
pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt);
float elevator = pidPitPit.update(
-pidSpdPit.update(
_guide->getAirSpeedCommand() - _nav->getAirSpeed(),
dt) - _nav->getPitch(), dt);
float rudder = pidYwrYaw.update(-_nav->getYawRate(), dt);
// desired yaw rate is zero, needs washout
float throttle = pidAltThr.update(
_guide->getAltitudeCommand() - _nav->getAlt(), dt);
// if needs trim
if (_needsTrim) {
// need to subtract current controller deflections so control
// surfaces are actually at the same position as manual flight
_ailTrim = _hal->rc[ch_roll]->getRadioPosition() - aileron;
_elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - elevator;
_rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - rudder;
_thrTrim = _hal->rc[ch_thr]->getRadioPosition() - throttle;
_needsTrim = false;
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
// actuator mixing/ output
_hal->rc[ch_roll]->setPosition(_aileron);
_hal->rc[ch_yaw]->setPosition(_rudder);
_hal->rc[ch_pitch]->setPosition(_elevator);
_hal->rc[ch_thrust]->setPosition(_throttle);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
// actuator mixing/ output
_hal->rc[ch_roll]->setPosition(
aileron + _rdrAilMix * rudder + _ailTrim);
_hal->rc[ch_yaw]->setPosition(rudder + _rdrTrim);
_hal->rc[ch_pitch]->setPosition(elevator + _elvTrim);
_hal->rc[ch_thr]->setPosition(throttle + _thrTrim);
//_hal->debug->println("automode");
// heading debug
// Serial.print("heading command: "); Serial.println(_guide->getHeadingCommand());
// Serial.print("heading: "); Serial.println(_nav->getYaw());
// Serial.print("heading error: "); Serial.println(headingError);
// alt debug
// Serial.print("alt command: "); Serial.println(_guide->getAltitudeCommand());
// Serial.print("alt: "); Serial.println(_nav->getAlt());
// Serial.print("alt error: "); Serial.println(_guide->getAltitudeCommand() - _nav->getAlt());
break;
}
default: {
setAllRadioChannelsToNeutral();
_mode = MAV_MODE_FAILSAFE;
_hal->setState(MAV_STATE_EMERGENCY);
_hal->debug->printf_P(PSTR("unknown controller mode\n"));
break;
}
}
}
private:
AP_Var_group _trimGroup;
AP_Uint8 _rdrAilMix;
bool _needsTrim;
AP_Float _ailTrim;
AP_Float _elvTrim;
AP_Float _rdrTrim;
AP_Float _thrTrim;
BlockPID pidBnkRll; // bank error to roll servo deflection
BlockPID pidSpdPit; // speed error to pitch command
BlockPID pidPitPit; // pitch error to pitch servo deflection
BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection
BlockPID pidHdgBnk; // heading error to bank command
BlockPID pidAltThr; // altitude error to throttle deflection
bool requireRadio;
float _aileron;
float _elevator;
float _rudder;
float _throttle;
};
} // namespace apo

127
apo/ControllerQuad.h
View File

@ -22,15 +22,15 @@ public:
* unique keys so they can be reaccessed from the hal rc vector
*/
enum {
CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order
CH_RIGHT,
CH_LEFT,
CH_FRONT,
CH_BACK,
CH_ROLL,
CH_PITCH,
CH_THRUST,
CH_YAW
ch_mode = 0, // note scicoslab channels set mode, left, right, front, back order
ch_right,
ch_left,
ch_front,
ch_back,
ch_roll,
ch_pitch,
ch_thrust,
ch_yaw
};
// must match channel enum
@ -60,7 +60,7 @@ public:
ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal),
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
@ -79,8 +79,10 @@ public:
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
_armingMechanism(hal,CH_THRUST,CH_YAW,0.1,-0.9,0.9), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) {
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
/*
* allocate radio channels
* the order of the channels has to match the enumeration above
@ -115,91 +117,30 @@ public:
1900, RC_MODE_IN, false));
}
virtual void update(const float & dt) {
//_hal->debug->printf_P(PSTR("thr: %f, yaw: %f\n"),_hal->rc[CH_THRUST]->getRadioPosition(),_hal->rc[CH_YAW]->getRadioPosition());
_armingMechanism.update(dt);
// determine flight mode
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
// manual/auto switch
} else {
// if all emergencies cleared, fall back to standby
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
else _mode = MAV_MODE_AUTO;
}
// handle flight modes
switch(_mode) {
case MAV_MODE_LOCKED: {
_hal->setState(MAV_STATE_STANDBY);
break;
}
case MAV_MODE_FAILSAFE: {
_hal->setState(MAV_STATE_EMERGENCY);
break;
}
case MAV_MODE_MANUAL: {
manualPositionLoop();
autoAttitudeLoop(dt);
break;
}
case MAV_MODE_AUTO: {
// until position loop is tested just
// go to standby
_hal->setState(MAV_STATE_STANDBY);
//attitudeLoop();
// XXX autoPositionLoop NOT TESTED, don't uncomment yet
//autoPositionLoop(dt);
//autoAttitudeLoop(dt);
break;
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY);
}
}
// this sends commands to motors
setMotors();
}
virtual MAV_MODE getMode() {
return (MAV_MODE) _mode.get();
}
private:
AP_Uint8 _mode;
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
AP_ArmingMechanism _armingMechanism;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
void manualPositionLoop() {
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_cmdRoll = -0.5 * _hal->rc[CH_ROLL]->getPosition();
_cmdPitch = -0.5 * _hal->rc[CH_PITCH]->getPosition();
_cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition();
_thrustMix = _hal->rc[CH_THRUST]->getPosition();
_cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition();
_cmdPitch = -0.5 * _hal->rc[ch_pitch]->getPosition();
_cmdYawRate = -1 * _hal->rc[ch_yaw]->getPosition();
_thrustMix = _hal->rc[ch_thrust]->getPosition();
autoAttitudeLoop(dt);
}
void autoLoop(const float dt) {
autoPositionLoop(dt);
autoAttitudeLoop(dt);
// XXX currently auto loop not tested, so
// put vehicle in standby
_hal->setState(MAV_STATE_STANDBY);
}
void autoPositionLoop(float dt) {
@ -242,13 +183,13 @@ private:
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) {
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[CH_RIGHT]->setPosition(_thrustMix - _rollMix + _yawMix);
_hal->rc[CH_LEFT]->setPosition(_thrustMix + _rollMix + _yawMix);
_hal->rc[CH_FRONT]->setPosition(_thrustMix + _pitchMix - _yawMix);
_hal->rc[CH_BACK]->setPosition(_thrustMix - _pitchMix - _yawMix);
_hal->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix);
_hal->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix);
_hal->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix);
_hal->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix);
}
break;
}

7
apo/PlaneEasystar.h
View File

@ -10,7 +10,7 @@
// vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE;
static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const uint8_t heartBeatTimeout = 3;
@ -117,6 +117,11 @@ static const float elvTrim = 0.0;
static const float rdrTrim = 0.0;
static const float thrTrim = 0.5;
// guidance
static const float velCmd = 1; // m/s
static const float xt = 10; // cross track gain
static const float xtLim = 90; // cross track angle limit, deg
#include "ControllerPlane.h"
#endif /* PLANEEASYSTAR_H_ */

8
apo/QuadArducopter.h
View File

@ -10,7 +10,7 @@
// vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const uint8_t heartBeatTimeout = 3;
@ -91,9 +91,13 @@ static const float PID_YAWSPEED_D = 0;
static const float PID_YAWSPEED_LIM = .05; // 5 % motors
static const float PID_YAWSPEED_AWU = 0.0;
static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
static const float THRUST_HOVER_OFFSET = 0.475;
// guidance
static const float velCmd = 1; // m/s
static const float xt = 10; // cross track gain
static const float xtLim = 90; // cross track angle limit, deg
#include "ControllerQuad.h"
#endif /* QUADARDUCOPTER_H_ */

2
libraries/APO/AP_Controller.cpp
View File

@ -24,7 +24,7 @@ AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
_group(key, name ? : PSTR("CNTRL_")),
_chMode(chMode),
_mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")) {
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
setAllRadioChannelsToNeutral();
}