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

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

This commit is contained in:
Wenyao Xie 2011-11-22 16:42:51 -05:00
parent d61e59d779
commit 40b3c303f7
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:
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),
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,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -49,7 +49,7 @@ private:
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -58,6 +58,10 @@ private:
_hal->rc[ch_str]->setPosition(_strCmd);
}
}
void handleFailsafe() {
// failsafe is to turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {

8
ArduRover/ControllerCar.h
View File

@ -16,7 +16,7 @@ class ControllerCar: public AP_Controller {
public:
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),
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,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -84,10 +84,14 @@ private:
_strCmd = steering;
_thrustCmd = thrust;
}
void setMotorsActive() {
void setMotors() {
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
}
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {

10
README.txt
View File

@ -44,9 +44,13 @@ Building using eclipse
Note: Unix can be substituted for MinGW/ MSYS/ NMake (for windows)
(see http://www.vtk.org/Wiki/Eclipse_CDT4_Generator)
PORT is the port for uploading to the board, COM0 etc on windows.
BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
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.
ARDUINO_SDK_PATH if it is not in default path can specify as /path/to/arduino
Importing the Eclipse Build Project:
Import project using Menu File->Import

10
apo/ControllerPlane.h
View File

@ -16,7 +16,7 @@ class ControllerPlane: public AP_Controller {
public:
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),
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_")),
_rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
_needsTrim(false),
@ -100,7 +100,7 @@ private:
_rudder += _rdrTrim;
_throttle += _thrTrim;
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -111,6 +111,12 @@ private:
_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
enum {

39
apo/ControllerQuad.h
View File

@ -18,7 +18,7 @@ class ControllerQuad: public AP_Controller {
public:
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),
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,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
@ -31,12 +31,10 @@ public:
pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_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),
pidTilt(new AP_Var_group(k_pidTilt, PSTR("TILT_")), 1, PID_TILT_P,
PID_TILT_I, PID_TILT_D, PID_TILT_AWU, PID_TILT_LIM, PID_TILT_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),
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),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
@ -91,16 +89,15 @@ private:
}
void autoPositionLoop(float dt) {
// XXX need to add waypoint coordinates
float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
//float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),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);
// "transform-to-body"
_cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw());
_cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw());
_cmdPitch *= -1; // note that the north tilt is negative of the pitch
_cmdPitch = -cmdTilt*cos(_guide->getHeadingError());
_cmdRoll = cmdTilt*sin(_guide->getHeadingError());
_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"
if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
@ -110,7 +107,7 @@ private:
else _thrustMix /= cos(_cmdPitch);
// 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) {
_rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@ -119,8 +116,7 @@ private:
_nav->getPitchRate(), dt);
_yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -132,6 +128,11 @@ private:
}
}
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
/**
* note that these are not the controller radio channel numbers, they are just
@ -163,8 +164,7 @@ private:
enum {
k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr,
k_pidPN,
k_pidPE,
k_pidTilt,
k_pidPD,
k_pidRoll,
k_pidPitch,
@ -173,7 +173,8 @@ private:
};
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
BlockPID pidTilt;
BlockPIDDfb pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
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;
// position control loop
static const float PID_POS_P = 0.1;
static const float PID_POS_I = 0;
static const float PID_POS_D = 0.1;
static const float PID_POS_LIM = 0.04; // about 2 deg
static const float PID_POS_AWU = 0.02; // about 1 deg
static const float PID_TILT_P = 0.1;
static const float PID_TILT_I = 0;
static const float PID_TILT_D = 0.1;
static const float PID_TILT_LIM = 0.04; // about 2 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_I = 0;
static const float PID_POS_Z_D = 0.2;
static const float PID_POS_Z_LIM = 0.1;
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
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_Controller.h"
#include "AP_RcChannel.h"
#include "../FastSerial/FastSerial.h"
#include "AP_HardwareAbstractionLayer.h"
@ -15,7 +16,7 @@ namespace apo {
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());
// arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) &&
if ( (_controller->getState() != MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
@ -23,14 +24,13 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock<0) _armingClock = 0;
if (_armingClock++ >= 100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
_hal->setState(MAV_STATE_ACTIVE);
_controller->setMode(MAV_MODE_READY);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
}
}
// disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &&
else if ( (_controller->getState() == MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
@ -38,8 +38,7 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock>0) _armingClock = 0;
if (_armingClock-- <= -100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
_hal->setState(MAV_STATE_STANDBY);
_controller->setMode(MAV_MODE_LOCKED);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
}
@ -47,8 +46,8 @@ void AP_ArmingMechanism::update(const float dt) {
// reset arming clock and report status
else if (_armingClock != 0) {
_armingClock = 0;
if (_hal->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"));
if (_controller->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
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 {
class AP_HardwareAbstractionLayer;
class AP_Controller;
class AP_ArmingMechanism {
@ -25,10 +26,10 @@ public:
* @param ch2Min: disarms below 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,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
_ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
float ch2Max) : _armingClock(0), _hal(hal), _controller(controller),
_ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
}
/**
@ -52,6 +53,7 @@ public:
private:
AP_HardwareAbstractionLayer * _hal;
AP_Controller * _controller;
int8_t _armingClock;
uint8_t _ch1; /// typically throttle 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),
callbackCalls(0) {
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@ -50,7 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
/*
* Calibration
*/
hal->setState(MAV_STATE_CALIBRATING);
controller->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
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.getCommand());
guide->setCurrentIndex(0);
controller->setMode(MAV_MODE_LOCKED);
controller->setState(MAV_STATE_STANDBY);
/*
* 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->gcs->sendText(SEVERITY_LOW, PSTR("running"));
hal->setState(MAV_STATE_STANDBY);
}
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;
}
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);
break;
}
@ -397,14 +397,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
break;
case MAV_ACTION_MOTORS_START:
_hal->setState(MAV_STATE_ACTIVE);
_controller->setMode(MAV_MODE_READY);
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
_hal->setState(MAV_STATE_STANDBY);
_controller->setMode(MAV_MODE_LOCKED);
_navigator->calibrate();
break;
@ -412,13 +412,11 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
_hal->setState(MAV_STATE_STANDBY);
_controller->setMode(MAV_MODE_LOCKED);
break;
case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF:
_controller->setMode(MAV_MODE_AUTO);
_guide->setMode(MAV_NAV_LIFTOFF);
break;
@ -651,6 +649,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
_receivingCmds = false;
_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"));
} else if (_cmdRequestIndex > _cmdNumberRequested) {
_receivingCmds = false;

100
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_LOCKED, PSTR("MODE")) {
_mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) {
setAllRadioChannelsToNeutral();
}
@ -45,83 +45,63 @@ void AP_Controller::setAllRadioChannelsManually() {
void AP_Controller::update(const float dt) {
if (_armingMechanism) _armingMechanism->update(dt);
// determine flight mode
// handle emergencies
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
setMode(MAV_MODE_FAILSAFE);
_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) {
setMode(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);
}
// handle modes
//
// 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 (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
if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(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: {
manualLoop(dt);
break;
}
case MAV_MODE_AUTO: {
autoLoop(dt);
break;
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY);
}
// handle all possible modes
if (getMode()==MAV_MODE_MANUAL) {
manualLoop(dt);
} else if (getMode()==MAV_MODE_AUTO) {
autoLoop(dt);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
setMode(MAV_MODE_FAILSAFE);
}
}
// this sends commands to motors
setMotors();
handleMotors();
}
void AP_Controller::setMotors() {
switch (_hal->getState()) {
case MAV_STATE_ACTIVE: {
void AP_Controller::handleMotors() {
if(getState()==MAV_STATE_ACTIVE) {
digitalWrite(_hal->aLedPin, HIGH);
setMotorsActive();
break;
}
case MAV_STATE_EMERGENCY: {
setMotors();
} else {
digitalWrite(_hal->aLedPin, LOW);
setMotorsEmergency();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setMotorsStandby();
break;
}
default: {
setMotorsStandby();
}
setAllRadioChannelsToNeutral();
}
}

40
libraries/APO/AP_Controller.h
View File

@ -44,29 +44,32 @@ public:
const uint8_t _chMode,
const uint16_t key = k_cntrl,
const prog_char_t * name = NULL);
virtual void update(const float dt);
// derived class cannot override
void update(const float dt);
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();
};
virtual void autoLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
AP_Uint8 getMode() {
void handleMotors();
// derived class must define
virtual void setMotors() = 0;
virtual void manualLoop(const float dt) = 0;
virtual void autoLoop(const float dt) = 0;
virtual void handleFailsafe() = 0;
// access
MAV_MODE getMode() {
return _mode;
}
void setMode(MAV_MODE mode) {
_mode = mode;
}
MAV_STATE getState() const {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
protected:
AP_Navigator * _nav;
@ -75,7 +78,8 @@ protected:
AP_ArmingMechanism * _armingMechanism;
uint8_t _chMode;
AP_Var_group _group;
AP_Uint8 _mode;
MAV_MODE _mode;
MAV_STATE _state;
};
class AP_ControllerBlock {
@ -218,7 +222,7 @@ public:
}
float update(const float & input, const float & dt) {
// 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
// because derivatve uses _eP as previous value
_eP = input;

10
libraries/APO/AP_Guide.cpp
View File

@ -42,6 +42,11 @@ float AP_Guide::getHeadingError() {
return headingError;
}
float AP_Guide::getDistanceToNextWaypoint() {
return _command.distanceTo(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal),
@ -151,12 +156,9 @@ void MavlinkGuide::handleCommand() {
return;
}
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
// check if we are at waypoint or if command
// 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->debug->printf_P(PSTR("waypoint reached (distance)"));
nextCommand();

2
libraries/APO/AP_Guide.h
View File

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

7
libraries/APO/AP_HardwareAbstractionLayer.h
View File

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