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

Further APO controller cleanup.

This commit is contained in:
James Goppert 2011-10-26 13:25:06 -04:00
parent 3ea6a4d287
commit 41463ade10
18 changed files with 332 additions and 434 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduBoat/ArduBoat.cpp
View File

@ -22,3 +22,4 @@
#include "APO_DefaultSetup.h"
#include <WProgram.h>; int main(void) {init();setup();for(;;) loop(); return 0; }
// vim:ts=4:sw=4:expandtab

1
ArduBoat/ArduBoat.pde
View File

@ -20,3 +20,4 @@
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"
// vim:ts=4:sw=4:expandtab

9
ArduBoat/BoatGeneric.h
View File

@ -28,10 +28,10 @@ static const uint8_t heartBeatTimeout = 3;
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors
static const bool gpsEnabled = false;
@ -84,3 +84,4 @@ static const float xtLim = 90;
#include "ControllerBoat.h"
#endif /* BOATGENERIC_H_ */
// vim:ts=4:sw=4:expandtab

142
ArduBoat/ControllerBoat.h
View File

@ -14,99 +14,67 @@ namespace apo {
class ControllerBoat: public AP_Controller {
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,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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0)
{
_hal->debug->println_P(PSTR("initializing boat controller"));
ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
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, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
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_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
_hal->rc.push_back(
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, 3, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
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();
}
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);
}
void setMotors() {
switch (_hal->getState()) {
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();
}
}
}
// methdos
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
}
void autoLoop(const float dt) {
_strCmd = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
void setMotorsActive() {
// 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);
}
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
};
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
};
} // namespace apo
#endif /* CONTROLLERBOAT_H_ */
// vim:ts=4:sw=4:expandtab

1
ArduRover/ArduRover.cpp
View File

@ -23,3 +23,4 @@
#include "APO_DefaultSetup.h"
#include <WProgram.h>; int main(void) {init();setup();for(;;) loop(); return 0; }
// vim:ts=4:sw=4:expandtab

9
ArduRover/CarStampede.h
View File

@ -28,10 +28,10 @@ static const uint8_t heartBeatTimeout = 3;
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors
static const bool gpsEnabled = false;
@ -84,3 +84,4 @@ static const float xtLim = 90;
#include "ControllerCar.h"
#endif /* CARSTAMPEDE_H_ */
// vim:ts=4:sw=4:expandtab

143
ArduRover/ControllerCar.h
View File

@ -14,100 +14,67 @@ namespace apo {
class ControllerCar: public AP_Controller {
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,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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
throttleDFCut), _strCmd(0), _thrustCmd(0)
{
_hal->debug->println_P(PSTR("initializing car controller"));
ControllerCar(AP_Navigator * nav, AP_Guide * guide,
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),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
throttleI, throttleD, throttleIMax, throttleYMax,
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, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
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_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
_hal->rc.push_back(
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, 3, 1100, 1500,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThrust, PSTR("THR_"), APM_RC, 2, 1100, 1500,
1900, RC_MODE_INOUT, false));
}
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();
}
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);
}
void setMotors() {
switch (_hal->getState()) {
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();
}
}
}
// methods
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_strCmd = _hal->rc[ch_str]->getRadioPosition();
_thrustCmd = _hal->rc[ch_thrust]->getRadioPosition();
}
void autoLoop(const float dt) {
_strCmd = pidStr.update(_guide->getHeadingError(), _nav->getYawRate(), dt);
_thrustCmd = pidThrust.update(
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
void setMotorsActive() {
// 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);
}
}
// attributes
enum {
ch_mode = 0, ch_str, ch_thrust
};
enum {
k_chMode = k_radioChannelsStart, k_chStr, k_chThrust
};
enum {
k_pidStr = k_controllersStart, k_pidThrust
};
BlockPIDDfb pidStr;
BlockPID pidThrust;
float _strCmd, _thrustCmd;
};
} // namespace apo
#endif /* CONTROLLERCAR_H_ */
// vim:ts=4:sw=4:expandtab

157
ArduRover/ControllerTank.h
View File

@ -14,101 +14,78 @@ namespace apo {
class ControllerTank: public AP_Controller {
public:
enum {
k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr
};
enum {
k_pidStr = k_controllersStart, k_pidThr
};
enum {
ch_mode = 0, ch_left, ch_right, ch_str, ch_thrust
};
ControllerTank(AP_Navigator * nav, AP_Guide * guide,
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"));
ControllerTank(AP_Navigator * nav, AP_Guide * guide,
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(
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_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500,
1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500,
1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500,
1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
1900, RC_MODE_IN, false));
}
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_headingOutput = _hal->rc[ch_str]->getPosition();
_throttleOutput = _hal->rc[ch_thrust]->getPosition();
}
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);
}
void setMotors() {
switch (_hal->getState()) {
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();
}
}
}
_hal->rc.push_back(
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_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500,
1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500,
1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500,
1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500,
1900, RC_MODE_IN, false));
}
private:
BlockPIDDfb pidStr;
BlockPID pidThr;
float _headingOutput;
float _throttleOutput;
// methods
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_headingOutput = _hal->rc[ch_str]->getPosition();
_throttleOutput = _hal->rc[ch_thrust]->getPosition();
}
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);
}
void setMotorsActive() {
// 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);
}
}
// attributes
enum {
k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr
};
enum {
k_pidStr = k_controllersStart, k_pidThr
};
enum {
ch_mode = 0, ch_left, ch_right, ch_str, ch_thrust
};
BlockPIDDfb pidStr;
BlockPID pidThr;
float _headingOutput;
float _throttleOutput;
};
} // namespace apo
#endif /* CONTROLLERTANK_H_ */
// vim:ts=4:sw=4:expandtab

9
ArduRover/TankGeneric.h
View File

@ -29,10 +29,10 @@ static const uint8_t heartBeatTimeout = 3;
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors
static const bool gpsEnabled = false;
@ -85,3 +85,4 @@ static const float xtLim = 90;
#include "ControllerTank.h"
#endif /* TANKGENERIC_H_ */
// vim:ts=4:sw=4:expandtab

103
apo/ControllerPlane.h
View File

@ -14,26 +14,6 @@ namespace apo {
class ControllerPlane: public AP_Controller {
public:
enum {
ch_mode = 0, ch_roll, ch_pitch, ch_thrust, ch_yaw
};
enum {
k_chMode = k_radioChannelsStart,
k_chRoll,
k_chPitch,
k_chYaw,
k_chThr,
k_pidBnkRll = k_controllersStart,
k_pidSpdPit,
k_pidPitPit,
k_pidYwrYaw,
k_pidHdgBnk,
k_pidAltThr,
k_trim = k_customStart
};
ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
@ -83,25 +63,17 @@ public:
1800, RC_MODE_INOUT, false));
}
private:
// methdos
void manualLoop(const float dt) {
setAllRadioChannelsManually();
// force auto to read new manual trim
if (_needsTrim == false)
_needsTrim = true;
//_hal->debug->println("manual");
}
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);
pidHdgBnk.update(_guide->getHeadingError(), dt) - _nav->getRoll(), dt);
_elevator = pidPitPit.update(
-pidSpdPit.update(
_guide->getAirSpeedCommand() - _nav->getAirSpeed(),
@ -112,7 +84,6 @@ public:
_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
@ -128,48 +99,39 @@ public:
_elevator += _elvTrim;
_rudder += _rdrTrim;
_throttle += _thrTrim;
//_hal->debug->println("automode");
}
void setMotorsActive() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_roll]->setPosition(_aileron);
_hal->rc[ch_yaw]->setPosition(_rudder);
_hal->rc[ch_pitch]->setPosition(_elevator);
_hal->rc[ch_thrust]->setPosition(_throttle);
}
}
void setMotors() {
// attributes
enum {
ch_mode = 0, ch_roll, ch_pitch, ch_thrust, ch_yaw
};
enum {
k_chMode = k_radioChannelsStart,
k_chRoll,
k_chPitch,
k_chYaw,
k_chThr,
switch (_hal->getState()) {
k_pidBnkRll = k_controllersStart,
k_pidSpdPit,
k_pidPitPit,
k_pidYwrYaw,
k_pidHdgBnk,
k_pidAltThr,
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();
}
}
}
private:
k_trim = k_customStart
};
AP_Var_group _trimGroup;
AP_Uint8 _rdrAilMix;
bool _needsTrim;
@ -193,3 +155,4 @@ private:
} // namespace apo
#endif /* CONTROLLERPLANE_H_ */
// vim:ts=4:sw=4:expandtab

139
apo/ControllerQuad.h
View File

@ -16,48 +16,6 @@ namespace apo {
class ControllerQuad: 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 {
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
enum {
k_chMode = k_radioChannelsStart,
k_chRight,
k_chLeft,
k_chFront,
k_chBack,
k_chRoll,
k_chPitch,
k_chThr,
k_chYaw
};
enum {
k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr,
k_pidPN,
k_pidPE,
k_pidPD,
k_pidRoll,
k_pidPitch,
k_pidYawRate,
k_pidYaw,
};
ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
@ -118,13 +76,7 @@ public:
}
private:
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
// methods
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition();
@ -133,7 +85,6 @@ private:
_thrustMix = _hal->rc[ch_thrust]->getPosition();
autoAttitudeLoop(dt);
}
void autoLoop(const float dt) {
autoPositionLoop(dt);
autoAttitudeLoop(dt);
@ -142,7 +93,6 @@ private:
// put vehicle in standby
_hal->setState(MAV_STATE_STANDBY);
}
void autoPositionLoop(float dt) {
float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
@ -167,7 +117,6 @@ private:
if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
else _thrustMix /= cos(_cmdPitch);
}
void autoAttitudeLoop(float dt) {
_rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
_nav->getRollRate(), dt);
@ -175,43 +124,65 @@ private:
_nav->getPitchRate(), dt);
_yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
}
void setMotors() {
switch (_hal->getState()) {
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_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;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
void setMotorsActive() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
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);
}
}
// attributes
/**
* 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 {
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
enum {
k_chMode = k_radioChannelsStart,
k_chRight,
k_chLeft,
k_chFront,
k_chBack,
k_chRoll,
k_chPitch,
k_chThr,
k_chYaw
};
enum {
k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr,
k_pidPN,
k_pidPE,
k_pidPD,
k_pidRoll,
k_pidPitch,
k_pidYawRate,
k_pidYaw,
};
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
};
} // namespace apo
#endif /* CONTROLLERQUAD_H_ */
// vim:ts=4:sw=4:expandtab

1
apo/PlaneEasystar.h
View File

@ -125,3 +125,4 @@ static const float xtLim = 90; // cross track angle limit, deg
#include "ControllerPlane.h"
#endif /* PLANEEASYSTAR_H_ */
// vim:ts=4:sw=4:expandtab

1
apo/QuadArducopter.h
View File

@ -101,3 +101,4 @@ static const float xtLim = 90; // cross track angle limit, deg
#include "ControllerQuad.h"
#endif /* QUADARDUCOPTER_H_ */
// vim:ts=4:sw=4:expandtab

4
apo/apo.pde
View File

@ -17,8 +17,8 @@
#include <WProgram.h>
// Vehicle Configuration
//#include "QuadArducopter.h"
#include "PlaneEasystar.h"
#include "QuadArducopter.h"
//#include "PlaneEasystar.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"

25
libraries/APO/AP_Controller.cpp
View File

@ -94,4 +94,29 @@ void AP_Controller::update(const float dt) {
setMotors();
}
void AP_Controller::setMotors() {
switch (_hal->getState()) {
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
setMotorsActive();
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setMotorsEmergency();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setMotorsStandby();
break;
}
default: {
setMotorsStandby();
}
}
}
} // namespace apo

9
libraries/APO/AP_Controller.h
View File

@ -45,9 +45,16 @@ public:
const uint16_t key = k_cntrl,
const prog_char_t * name = NULL);
virtual void update(const float dt);
virtual void setMotors() = 0;
void setAllRadioChannelsToNeutral();
void setAllRadioChannelsManually();
virtual void setMotors();
virtual void setMotorsActive() = 0;
virtual void setMotorsEmergency() {
setAllRadioChannelsToNeutral();
};
virtual void setMotorsStandby() {
setAllRadioChannelsToNeutral();
};
virtual void manualLoop(const float dt) {
setAllRadioChannelsToNeutral();
};

10
libraries/APO/AP_Guide.cpp
View File

@ -31,6 +31,16 @@ void AP_Guide::setCurrentIndex(uint8_t val) {
_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT);
}
float AP_Guide::getHeadingError() {
float headingError = getHeadingCommand()
- _navigator->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
return headingError;
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),

2
libraries/APO/AP_Guide.h
View File

@ -76,6 +76,8 @@ public:
return nextIndex;
}
float getHeadingError();
float getHeadingCommand() {
return _headingCommand;
}