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

Worked on autopilot state/ mode mapping. Corrected PIDBlock sign error.

Browse Source
This commit is contained in:
Wenyao Xie 2011-11-22 16:42:51 -05:00
parent 50e8de999f
commit 3c217d4186
15 changed files with 150 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduBoat/ControllerBoat.h
View File

@ -16,7 +16,7 @@ class ControllerBoat: public AP_Controller {
public: public:
ControllerBoat(AP_Navigator * nav, AP_Guide * guide, ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) : AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode), AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -49,7 +49,7 @@ private:
_guide->getGroundSpeedCommand() _guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt); - _nav->getGroundSpeed(), dt);
} }
void setMotorsActive() { void setMotors() {
// turn all motors off if below 0.1 throttle // turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral(); setAllRadioChannelsToNeutral();
@ -58,6 +58,10 @@ private:
_hal->rc[ch_str]->setPosition(_strCmd); _hal->rc[ch_str]->setPosition(_strCmd);
} }
} }
void handleFailsafe() {
// failsafe is to turn off
setMode(MAV_MODE_LOCKED);
}
// attributes // attributes
enum { enum {

8
ArduRover/ControllerCar.h
View File

@ -16,7 +16,7 @@ class ControllerCar: public AP_Controller {
public: public:
ControllerCar(AP_Navigator * nav, AP_Guide * guide, ControllerCar(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) : AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode), AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP, pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -84,10 +84,14 @@ private:
_strCmd = steering; _strCmd = steering;
_thrustCmd = thrust; _thrustCmd = thrust;
} }
void setMotorsActive() { void setMotors() {
_hal->rc[ch_str]->setPosition(_strCmd); _hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd); _hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
} }
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes // attributes
enum { enum {

6
README.txt
View File

@ -44,8 +44,12 @@ Building using eclipse
Note: Unix can be substituted for MinGW/ MSYS/ NMake (for windows) Note: Unix can be substituted for MinGW/ MSYS/ NMake (for windows)
(see http://www.vtk.org/Wiki/Eclipse_CDT4_Generator) (see http://www.vtk.org/Wiki/Eclipse_CDT4_Generator)
PORT is the port for uploading to the board, COM0 etc on windows. input options:
CMAKE_BUILD_TYPE choose from DEBUG, RELEASE etc.
PORT is the port for uploading to the board, COM0 etc on windows. /dev/ttyUSB0 etc. on linux
BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards. BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
ARDUINO_SDK_PATH if it is not in default path can specify as /path/to/arduino
Importing the Eclipse Build Project: Importing the Eclipse Build Project:

10
apo/ControllerPlane.h
View File

@ -16,7 +16,7 @@ class ControllerPlane: public AP_Controller {
public: public:
ControllerPlane(AP_Navigator * nav, AP_Guide * guide, ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) : AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode), AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
_trimGroup(k_trim, PSTR("trim_")), _trimGroup(k_trim, PSTR("trim_")),
_rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
_needsTrim(false), _needsTrim(false),
@ -100,7 +100,7 @@ private:
_rudder += _rdrTrim; _rudder += _rdrTrim;
_throttle += _thrTrim; _throttle += _thrTrim;
} }
void setMotorsActive() { void setMotors() {
// turn all motors off if below 0.1 throttle // turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral(); setAllRadioChannelsToNeutral();
@ -111,6 +111,12 @@ private:
_hal->rc[ch_thrust]->setPosition(_throttle); _hal->rc[ch_thrust]->setPosition(_throttle);
} }
} }
void handleFailsafe() {
// note if testing and communication is lost the motors will not shut off,
// it will attempt to land
_guide->setMode(MAV_NAV_LANDING);
setMode(MAV_MODE_AUTO);
}
// attributes // attributes
enum { enum {

39
apo/ControllerQuad.h
View File

@ -18,7 +18,7 @@ class ControllerQuad: public AP_Controller {
public: public:
ControllerQuad(AP_Navigator * nav, AP_Guide * guide, ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) : AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode), AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT), PID_ATT_LIM, PID_ATT_DFCUT),
@ -31,12 +31,10 @@ public:
pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1, pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT), PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P, pidTilt(new AP_Var_group(k_pidTilt, PSTR("TILT_")), 1, PID_TILT_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT), PID_TILT_I, PID_TILT_D, PID_TILT_AWU, PID_TILT_LIM, PID_TILT_DFCUT),
pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
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, 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), PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_Z_DFCUT),
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) { _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller")); _hal->debug->println_P(PSTR("initializing quad controller"));
@ -91,16 +89,15 @@ private:
} }
void autoPositionLoop(float dt) { void autoPositionLoop(float dt) {
// XXX need to add waypoint coordinates // XXX need to add waypoint coordinates
float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt); //float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt); //float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt); float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
// "transform-to-body" _cmdPitch = -cmdTilt*cos(_guide->getHeadingError());
_cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw()); _cmdRoll = cmdTilt*sin(_guide->getHeadingError());
_cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw());
_cmdPitch *= -1; // note that the north tilt is negative of the pitch
_cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint
_thrustMix = THRUST_HOVER_OFFSET + cmdDown; _thrustMix = THRUST_HOVER_OFFSET - cmdDown;
// "thrust-trim-adjust" // "thrust-trim-adjust"
if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393; if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
@ -110,7 +107,7 @@ private:
else _thrustMix /= cos(_cmdPitch); else _thrustMix /= cos(_cmdPitch);
// debug for position loop // debug for position loop
_hal->debug->printf_P(PSTR("cmd: north tilt(%f), east tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdNorthTilt,cmdEastTilt,cmdDown,_cmdPitch,_cmdRoll); _hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll);
} }
void autoAttitudeLoop(float dt) { void autoAttitudeLoop(float dt) {
_rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@ -119,8 +116,7 @@ private:
_nav->getPitchRate(), dt); _nav->getPitchRate(), dt);
_yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
} }
void setMotorsActive() { void setMotors() {
// turn all motors off if below 0.1 throttle // turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral(); setAllRadioChannelsToNeutral();
@ -132,6 +128,11 @@ private:
} }
} }
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes // attributes
/** /**
* note that these are not the controller radio channel numbers, they are just * note that these are not the controller radio channel numbers, they are just
@ -163,8 +164,7 @@ private:
enum { enum {
k_pidGroundSpeed2Throttle = k_controllersStart, k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr, k_pidStr,
k_pidPN, k_pidTilt,
k_pidPE,
k_pidPD, k_pidPD,
k_pidRoll, k_pidRoll,
k_pidPitch, k_pidPitch,
@ -173,7 +173,8 @@ private:
}; };
BlockPIDDfb pidRoll, pidPitch, pidYaw; BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate; BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD; BlockPID pidTilt;
BlockPIDDfb pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix; float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate; float _cmdRoll, _cmdPitch, _cmdYawRate;
}; };

13
apo/QuadArducopter.h
View File

@ -61,17 +61,18 @@ static const float loop2Rate = 1; // gcs slow
static const float loop3Rate = 0.1; static const float loop3Rate = 0.1;
// position control loop // position control loop
static const float PID_POS_P = 0.1; static const float PID_TILT_P = 0.1;
static const float PID_POS_I = 0; static const float PID_TILT_I = 0;
static const float PID_POS_D = 0.1; static const float PID_TILT_D = 0.1;
static const float PID_POS_LIM = 0.04; // about 2 deg static const float PID_TILT_LIM = 0.04; // about 2 deg
static const float PID_POS_AWU = 0.02; // about 1 deg static const float PID_TILT_AWU = 0.02; // about 1 deg
static const float PID_TILT_DFCUT = 10; // cut derivative feedback at 10 hz
static const float PID_POS_Z_P = 0.1; static const float PID_POS_Z_P = 0.1;
static const float PID_POS_Z_I = 0; static const float PID_POS_Z_I = 0;
static const float PID_POS_Z_D = 0.2; static const float PID_POS_Z_D = 0.2;
static const float PID_POS_Z_LIM = 0.1; static const float PID_POS_Z_LIM = 0.1;
static const float PID_POS_Z_AWU = 0; static const float PID_POS_Z_AWU = 0;
static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz static const float PID_POS_Z_DFCUT = 10; // cut derivative feedback at 10 hz
// attitude control loop // attitude control loop
static const float PID_ATT_P = 0.17; static const float PID_ATT_P = 0.17;

15
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -5,6 +5,7 @@
#include "AP_ArmingMechanism.h" #include "AP_ArmingMechanism.h"
#include "AP_Controller.h"
#include "AP_RcChannel.h" #include "AP_RcChannel.h"
#include "../FastSerial/FastSerial.h" #include "../FastSerial/FastSerial.h"
#include "AP_HardwareAbstractionLayer.h" #include "AP_HardwareAbstractionLayer.h"
@ -15,7 +16,7 @@ namespace apo {
void AP_ArmingMechanism::update(const float dt) { void AP_ArmingMechanism::update(const float dt) {
//_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition()); //_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition());
// arming // arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) && if ( (_controller->getState() != MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
@ -23,14 +24,13 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock<0) _armingClock = 0; if (_armingClock<0) _armingClock = 0;
if (_armingClock++ >= 100) { if (_armingClock++ >= 100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); _controller->setMode(MAV_MODE_READY);
_hal->setState(MAV_STATE_ACTIVE);
} else { } else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
} }
} }
// disarming // disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) && else if ( (_controller->getState() == MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
@ -38,8 +38,7 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock>0) _armingClock = 0; if (_armingClock>0) _armingClock = 0;
if (_armingClock-- <= -100) { if (_armingClock-- <= -100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); _controller->setMode(MAV_MODE_LOCKED);
_hal->setState(MAV_STATE_STANDBY);
} else { } else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
} }
@ -47,8 +46,8 @@ void AP_ArmingMechanism::update(const float dt) {
// reset arming clock and report status // reset arming clock and report status
else if (_armingClock != 0) { else if (_armingClock != 0) {
_armingClock = 0; _armingClock = 0;
if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); if (_controller->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); else if (_controller->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
} }
} }

8
libraries/APO/AP_ArmingMechanism.h
View File

@ -12,6 +12,7 @@
namespace apo { namespace apo {
class AP_HardwareAbstractionLayer; class AP_HardwareAbstractionLayer;
class AP_Controller;
class AP_ArmingMechanism { class AP_ArmingMechanism {
@ -25,10 +26,10 @@ public:
* @param ch2Min: disarms below this * @param ch2Min: disarms below this
* @param ch2Max: arms above this * @param ch2Max: arms above this
*/ */
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, AP_Controller * controller,
uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), float ch2Max) : _armingClock(0), _hal(hal), _controller(controller),
_ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { _ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
} }
/** /**
@ -52,6 +53,7 @@ public:
private: private:
AP_HardwareAbstractionLayer * _hal; AP_HardwareAbstractionLayer * _hal;
AP_Controller * _controller;
int8_t _armingClock; int8_t _armingClock;
uint8_t _ch1; /// typically throttle channel uint8_t _ch1; /// typically throttle channel
uint8_t _ch2; /// typically yaw channel uint8_t _ch2; /// typically yaw channel

6
libraries/APO/AP_Autopilot.cpp
View File

@ -37,7 +37,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
_controller(controller), _hal(hal), _controller(controller), _hal(hal),
callbackCalls(0) { callbackCalls(0) {
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@ -50,7 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
/* /*
* Calibration * Calibration
*/ */
hal->setState(MAV_STATE_CALIBRATING); controller->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@ -110,6 +109,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
AP_MavlinkCommand::home.getLon()*rad2Deg, AP_MavlinkCommand::home.getLon()*rad2Deg,
AP_MavlinkCommand::home.getCommand()); AP_MavlinkCommand::home.getCommand());
guide->setCurrentIndex(0); guide->setCurrentIndex(0);
controller->setMode(MAV_MODE_LOCKED);
controller->setState(MAV_STATE_STANDBY);
/* /*
* Attach loops, stacking for priority * Attach loops, stacking for priority
@ -122,7 +123,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
hal->debug->println_P(PSTR("running")); hal->debug->println_P(PSTR("running"));
hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
hal->setState(MAV_STATE_STANDBY);
} }
void AP_Autopilot::callback(void * data) { void AP_Autopilot::callback(void * data) {

16
libraries/APO/AP_CommLink.cpp
View File

@ -183,7 +183,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
batteryVoltage = _hal->batteryMonitor->getVoltage()*1000; batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
} }
mavlink_msg_sys_status_send(_channel, _controller->getMode(), mavlink_msg_sys_status_send(_channel, _controller->getMode(),
_guide->getMode(), _hal->getState(), _hal->load * 10, _guide->getMode(), _controller->getState(), _hal->load * 10,
batteryVoltage, batteryPercentage, _packetDrops); batteryVoltage, batteryPercentage, _packetDrops);
break; break;
} }
@ -397,14 +397,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
break; break;
case MAV_ACTION_MOTORS_START: case MAV_ACTION_MOTORS_START:
_hal->setState(MAV_STATE_ACTIVE); _controller->setMode(MAV_MODE_READY);
break; break;
case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_CALIBRATE_PRESSURE:
_hal->setState(MAV_STATE_STANDBY); _controller->setMode(MAV_MODE_LOCKED);
_navigator->calibrate(); _navigator->calibrate();
break; break;
@ -412,13 +412,11 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN: case MAV_ACTION_SHUTDOWN:
_hal->setState(MAV_STATE_STANDBY);
_controller->setMode(MAV_MODE_LOCKED); _controller->setMode(MAV_MODE_LOCKED);
break; break;
case MAV_ACTION_LAUNCH: case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF: case MAV_ACTION_TAKEOFF:
_controller->setMode(MAV_MODE_AUTO);
_guide->setMode(MAV_NAV_LIFTOFF); _guide->setMode(MAV_NAV_LIFTOFF);
break; break;
@ -651,6 +649,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
_receivingCmds = false; _receivingCmds = false;
_guide->setNumberOfCommands(_cmdNumberRequested); _guide->setNumberOfCommands(_cmdNumberRequested);
// make sure curernt waypoint still exists
if (_cmdNumberRequested > _guide->getCurrentIndex()) {
_guide->setCurrentIndex(0);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
}
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); //sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
} else if (_cmdRequestIndex > _cmdNumberRequested) { } else if (_cmdRequestIndex > _cmdNumberRequested) {
_receivingCmds = false; _receivingCmds = false;

90
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), _nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
_group(key, name ? : PSTR("CNTRL_")), _group(key, name ? : PSTR("CNTRL_")),
_chMode(chMode), _chMode(chMode),
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) { _mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) {
setAllRadioChannelsToNeutral(); setAllRadioChannelsToNeutral();
} }
@ -45,83 +45,63 @@ void AP_Controller::setAllRadioChannelsManually() {
void AP_Controller::update(const float dt) { void AP_Controller::update(const float dt) {
if (_armingMechanism) _armingMechanism->update(dt); if (_armingMechanism) _armingMechanism->update(dt);
// determine flight mode // handle emergencies
// //
// check for heartbeat from gcs, if not found go to failsafe // check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) { if (_hal->heartBeatLost()) {
setMode(MAV_MODE_FAILSAFE); setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe // if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) { } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
setMode(MAV_MODE_FAILSAFE); setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
// manual/auto switch }
} else {
// if all emergencies cleared, fall back to standby // handle modes
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY); //
// if in locked mode
if (getMode() == MAV_MODE_LOCKED) {
// if state is not stanby then set it to standby and alert gcs
if (getState()!=MAV_STATE_STANDBY) {
setState(MAV_STATE_STANDBY);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}
// if not locked
else {
// if state is not active, set it to active and alert gcs
if (getState()!=MAV_STATE_ACTIVE) {
setState(MAV_STATE_ACTIVE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
}
// if in auto mode and manual switch set, change to manual // if in auto mode and manual switch set, change to manual
if (getMode()==MAV_MODE_AUTO && (_hal->rc[_chMode]->getRadioPosition() > 0)) setMode(MAV_MODE_MANUAL); if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL);
else setMode(MAV_MODE_AUTO);
// if in manual mode and auto switch set, switch to auto // handle all possible modes
if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(MAV_MODE_AUTO); if (getMode()==MAV_MODE_MANUAL) {
}
// 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: {
manualLoop(dt); manualLoop(dt);
break; } else if (getMode()==MAV_MODE_AUTO) {
}
case MAV_MODE_AUTO: {
autoLoop(dt); autoLoop(dt);
break; } else {
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY); setMode(MAV_MODE_FAILSAFE);
} }
} }
// this sends commands to motors // this sends commands to motors
setMotors(); handleMotors();
} }
void AP_Controller::setMotors() { void AP_Controller::handleMotors() {
switch (_hal->getState()) { if(getState()==MAV_STATE_ACTIVE) {
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH); digitalWrite(_hal->aLedPin, HIGH);
setMotorsActive(); setMotors();
break; } else {
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW); digitalWrite(_hal->aLedPin, LOW);
setMotorsEmergency(); setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setMotorsStandby();
break;
}
default: {
setMotorsStandby();
}
} }
} }

40
libraries/APO/AP_Controller.h
View File

@ -44,29 +44,32 @@ public:
const uint8_t _chMode, const uint8_t _chMode,
const uint16_t key = k_cntrl, const uint16_t key = k_cntrl,
const prog_char_t * name = NULL); const prog_char_t * name = NULL);
virtual void update(const float dt);
// derived class cannot override
void update(const float dt);
void setAllRadioChannelsToNeutral(); void setAllRadioChannelsToNeutral();
void setAllRadioChannelsManually(); void setAllRadioChannelsManually();
virtual void setMotors(); void handleMotors();
virtual void setMotorsActive() = 0;
virtual void setMotorsEmergency() { // derived class must define
setAllRadioChannelsToNeutral(); virtual void setMotors() = 0;
}; virtual void manualLoop(const float dt) = 0;
virtual void setMotorsStandby() { virtual void autoLoop(const float dt) = 0;
setAllRadioChannelsToNeutral(); virtual void handleFailsafe() = 0;
};
virtual void manualLoop(const float dt) { // access
setAllRadioChannelsToNeutral(); MAV_MODE getMode() {
};
virtual void autoLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
AP_Uint8 getMode() {
return _mode; return _mode;
} }
void setMode(MAV_MODE mode) { void setMode(MAV_MODE mode) {
_mode = mode; _mode = mode;
} }
MAV_STATE getState() const {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
protected: protected:
AP_Navigator * _nav; AP_Navigator * _nav;
@ -75,7 +78,8 @@ protected:
AP_ArmingMechanism * _armingMechanism; AP_ArmingMechanism * _armingMechanism;
uint8_t _chMode; uint8_t _chMode;
AP_Var_group _group; AP_Var_group _group;
AP_Uint8 _mode; MAV_MODE _mode;
MAV_STATE _state;
}; };
class AP_ControllerBlock { class AP_ControllerBlock {
@ -218,7 +222,7 @@ public:
} }
float update(const float & input, const float & dt) { float update(const float & input, const float & dt) {
// derivative with low pass // derivative with low pass
float _eD = _blockLowPass.update((_eP - input) / dt, dt); float _eD = _blockLowPass.update((input - _eP) / dt, dt);
// proportional, note must come after derivative // proportional, note must come after derivative
// because derivatve uses _eP as previous value // because derivatve uses _eP as previous value
_eP = input; _eP = input;

10
libraries/APO/AP_Guide.cpp
View File

@ -42,6 +42,11 @@ float AP_Guide::getHeadingError() {
return headingError; return headingError;
} }
float AP_Guide::getDistanceToNextWaypoint() {
return _command.distanceTo(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator, MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) : AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), AP_Guide(navigator, hal),
@ -151,12 +156,9 @@ void MavlinkGuide::handleCommand() {
return; return;
} }
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
// check if we are at waypoint or if command // check if we are at waypoint or if command
// radius is zero within tolerance // radius is zero within tolerance
if ( (distanceToNext < _command.getRadius()) | (distanceToNext < 1e-5) ) { if ( (getDistanceToNextWaypoint() < _command.getRadius()) | (getDistanceToNextWaypoint() < 1e-5) ) {
_hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)"));
_hal->debug->printf_P(PSTR("waypoint reached (distance)")); _hal->debug->printf_P(PSTR("waypoint reached (distance)"));
nextCommand(); nextCommand();

2
libraries/APO/AP_Guide.h
View File

@ -97,6 +97,8 @@ public:
float getAltitudeCommand() { float getAltitudeCommand() {
return _altitudeCommand; return _altitudeCommand;
} }
float getDistanceToNextWaypoint();
virtual float getPNError() = 0; virtual float getPNError() = 0;
virtual float getPEError() = 0; virtual float getPEError() = 0;
virtual float getPDError() = 0; virtual float getPDError() = 0;

7
libraries/APO/AP_HardwareAbstractionLayer.h
View File

@ -146,13 +146,6 @@ public:
vehicle_t getVehicle() { vehicle_t getVehicle() {
return _vehicle; return _vehicle;
} }
MAV_STATE getState() {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
bool heartBeatLost() { bool heartBeatLost() {
if (_heartBeatTimeout == 0) if (_heartBeatTimeout == 0)
return false; return false;