diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 2b222a7ee0..2e32a371e7 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -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 { diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 75a2b76a73..2a244f1cfd 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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 { diff --git a/README.txt b/README.txt index 0e1ad4a89b..c72fe9aa77 100644 --- a/README.txt +++ b/README.txt @@ -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 diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 368a323020..3c9d6e56c6 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -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 { diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 8f728f08d2..0923f2a691 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -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; }; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 785fee689e..cda3666c84 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -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; diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index f319a6d91e..bb6bf22a53 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -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")); } } diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h index e747a7831e..8ff3e56105 100644 --- a/libraries/APO/AP_ArmingMechanism.h +++ b/libraries/APO/AP_ArmingMechanism.h @@ -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 diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index a2f79fe1f6..858695ad3d 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -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) { diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 6be0bf1842..9ccd483ee2 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -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; diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 73acf7c066..9925923f59 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -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(); } } diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index d16fe48052..639ec22b71 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -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; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index c63cad4048..9b365d8384 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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(); diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 2f2d80ec89..fbb6b055f2 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -97,6 +97,8 @@ public: float getAltitudeCommand() { return _altitudeCommand; } + float getDistanceToNextWaypoint(); + virtual float getPNError() = 0; virtual float getPEError() = 0; virtual float getPDError() = 0; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 9b3b9e2810..14c9b8ad1c 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -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;