From d50d0682e9a67abe61cad9eb18cce1ae0a19fba5 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Fri, 26 Oct 2012 19:48:03 -0700 Subject: [PATCH] remove APO library: deprecated --- apo/.gitignore | 1 - apo/ControllerPlane.h | 164 ---- apo/ControllerQuad.h | 192 ----- apo/Makefile | 1 - apo/PlaneEasystar.h | 102 --- apo/QuadArducopter.h | 109 --- apo/apo.pde | 24 - libraries/APO/APO.cpp | 8 - libraries/APO/APO.h | 34 - libraries/APO/APO_DefaultSetup.h | 46 - libraries/APO/AP_ArmingMechanism.cpp | 55 -- libraries/APO/AP_ArmingMechanism.h | 69 -- libraries/APO/AP_Autopilot.cpp | 273 ------ libraries/APO/AP_Autopilot.h | 123 --- libraries/APO/AP_BatteryMonitor.cpp | 12 - libraries/APO/AP_BatteryMonitor.h | 52 -- libraries/APO/AP_Board.cpp | 14 - libraries/APO/AP_Board.h | 139 --- libraries/APO/AP_CommLink.cpp | 800 ------------------ libraries/APO/AP_CommLink.h | 141 --- libraries/APO/AP_Controller.cpp | 106 --- libraries/APO/AP_Controller.h | 148 ---- libraries/APO/AP_ControllerBlock.cpp | 176 ---- libraries/APO/AP_ControllerBlock.h | 235 ----- libraries/APO/AP_Guide.cpp | 236 ------ libraries/APO/AP_Guide.h | 167 ---- libraries/APO/AP_Loop.cpp | 52 -- libraries/APO/AP_Loop.h | 61 -- libraries/APO/AP_MavlinkCommand.cpp | 212 ----- libraries/APO/AP_MavlinkCommand.h | 380 --------- libraries/APO/AP_Navigator.cpp | 45 - libraries/APO/AP_Navigator.h | 297 ------- libraries/APO/AP_RcChannel.cpp | 103 --- libraries/APO/AP_RcChannel.h | 85 -- libraries/APO/AP_Var_keys.h | 27 - libraries/APO/Board_APM1.cpp | 186 ---- libraries/APO/Board_APM1.h | 24 - libraries/APO/Board_APM1_2560.cpp | 14 - libraries/APO/Board_APM1_2560.h | 26 - libraries/APO/Board_APM2.cpp | 187 ---- libraries/APO/Board_APM2.h | 24 - libraries/APO/Navigator_Dcm.cpp | 220 ----- libraries/APO/Navigator_Dcm.h | 51 -- libraries/APO/constants.h | 24 - libraries/APO/examples/MavlinkTest/Makefile | 2 - .../APO/examples/MavlinkTest/MavlinkTest.pde | 49 -- libraries/APO/examples/ServoManual/Makefile | 2 - .../APO/examples/ServoManual/ServoManual.pde | 112 --- libraries/APO/examples/ServoSweep/Makefile | 2 - .../APO/examples/ServoSweep/ServoSweep.pde | 125 --- libraries/APO/template.h | 32 - 51 files changed, 5769 deletions(-) delete mode 100644 apo/.gitignore delete mode 100644 apo/ControllerPlane.h delete mode 100644 apo/ControllerQuad.h delete mode 100644 apo/Makefile delete mode 100644 apo/PlaneEasystar.h delete mode 100644 apo/QuadArducopter.h delete mode 100644 apo/apo.pde delete mode 100644 libraries/APO/APO.cpp delete mode 100644 libraries/APO/APO.h delete mode 100644 libraries/APO/APO_DefaultSetup.h delete mode 100644 libraries/APO/AP_ArmingMechanism.cpp delete mode 100644 libraries/APO/AP_ArmingMechanism.h delete mode 100644 libraries/APO/AP_Autopilot.cpp delete mode 100644 libraries/APO/AP_Autopilot.h delete mode 100644 libraries/APO/AP_BatteryMonitor.cpp delete mode 100644 libraries/APO/AP_BatteryMonitor.h delete mode 100644 libraries/APO/AP_Board.cpp delete mode 100644 libraries/APO/AP_Board.h delete mode 100644 libraries/APO/AP_CommLink.cpp delete mode 100644 libraries/APO/AP_CommLink.h delete mode 100644 libraries/APO/AP_Controller.cpp delete mode 100644 libraries/APO/AP_Controller.h delete mode 100644 libraries/APO/AP_ControllerBlock.cpp delete mode 100644 libraries/APO/AP_ControllerBlock.h delete mode 100644 libraries/APO/AP_Guide.cpp delete mode 100644 libraries/APO/AP_Guide.h delete mode 100644 libraries/APO/AP_Loop.cpp delete mode 100644 libraries/APO/AP_Loop.h delete mode 100644 libraries/APO/AP_MavlinkCommand.cpp delete mode 100644 libraries/APO/AP_MavlinkCommand.h delete mode 100644 libraries/APO/AP_Navigator.cpp delete mode 100644 libraries/APO/AP_Navigator.h delete mode 100644 libraries/APO/AP_RcChannel.cpp delete mode 100644 libraries/APO/AP_RcChannel.h delete mode 100644 libraries/APO/AP_Var_keys.h delete mode 100644 libraries/APO/Board_APM1.cpp delete mode 100644 libraries/APO/Board_APM1.h delete mode 100644 libraries/APO/Board_APM1_2560.cpp delete mode 100644 libraries/APO/Board_APM1_2560.h delete mode 100644 libraries/APO/Board_APM2.cpp delete mode 100644 libraries/APO/Board_APM2.h delete mode 100644 libraries/APO/Navigator_Dcm.cpp delete mode 100644 libraries/APO/Navigator_Dcm.h delete mode 100644 libraries/APO/constants.h delete mode 100644 libraries/APO/examples/MavlinkTest/Makefile delete mode 100644 libraries/APO/examples/MavlinkTest/MavlinkTest.pde delete mode 100644 libraries/APO/examples/ServoManual/Makefile delete mode 100644 libraries/APO/examples/ServoManual/ServoManual.pde delete mode 100644 libraries/APO/examples/ServoSweep/Makefile delete mode 100644 libraries/APO/examples/ServoSweep/ServoSweep.pde delete mode 100644 libraries/APO/template.h diff --git a/apo/.gitignore b/apo/.gitignore deleted file mode 100644 index e31d5d5b64..0000000000 --- a/apo/.gitignore +++ /dev/null @@ -1 +0,0 @@ -apo.cpp diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h deleted file mode 100644 index 640e2d232b..0000000000 --- a/apo/ControllerPlane.h +++ /dev/null @@ -1,164 +0,0 @@ -/* - * ControllerPlane.h - * - * Created on: Jun 30, 2011 - * Author: jgoppert - */ - -#ifndef CONTROLLERPLANE_H_ -#define CONTROLLERPLANE_H_ - -#include "../APO/AP_Controller.h" - -namespace apo { - -class ControllerPlane: public AP_Controller { -public: - ControllerPlane(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board) : - AP_Controller(nav, guide, board, new AP_ArmingMechanism(board,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl), - _trimGroup(k_trim, PSTR("trim_")), - _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), - _needsTrim(false), - _ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")), - _elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")), - _rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")), - _thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")), - pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1, - pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu, - pidBnkRllLim, pidBnkRllDFCut), - pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1, - pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu, - pidPitPitLim, pidPitPitDFCut), - pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1, - pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu, - pidSpdPitLim, pidSpdPitDFCut), - pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1, - pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu, - pidYwrYawLim, pidYwrYawDFCut), - pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1, - pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu, - pidHdgBnkLim, pidHdgBnkDFCut), - pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1, - pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu, - pidAltThrLim, pidAltThrDFCut), - requireRadio(false), _aileron(0), _elevator(0), _rudder(0), _throttle(0) { - - _board->debug->println_P(PSTR("initializing plane controller")); - - _board->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("mode_"), board->radio, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("roll_"), board->radio, 0, 1200, - 1500, 1800, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("pitch_"), board->radio, 1, 1200, - 1500, 1800, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("thr_"), board->radio, 2, 1100, 1100, - 1900, RC_MODE_INOUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("yaw_"), board->radio, 3, 1200, 1500, - 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; - } - void autoLoop(const float dt) { - _aileron = pidBnkRll.update( - pidHdgBnk.update(_guide->getHeadingError(), dt) - _nav->getRoll(), dt); - _elevator = pidPitPit.update( - -pidSpdPit.update( - _guide->getAirSpeedCommand() - _nav->getAirSpeed(), - dt) - _nav->getPitch(), dt); - _rudder = pidYwrYaw.update(-_nav->getYawRate(), dt); - - // desired yaw rate is zero, needs washout - _throttle = pidAltThr.update( - _guide->getAltitudeCommand() - _nav->getAlt(), dt); - - if (_needsTrim) { - // need to subtract current controller deflections so control - // surfaces are actually at the same position as manual flight - _ailTrim = _board->rc[ch_roll]->getRadioPosition() - _aileron; - _elvTrim = _board->rc[ch_pitch]->getRadioPosition() - _elevator; - _rdrTrim = _board->rc[ch_yaw]->getRadioPosition() - _rudder; - _thrTrim = _board->rc[ch_thrust]->getRadioPosition() - _throttle; - _needsTrim = false; - } - - // actuator mixing/ output - _aileron += _rdrAilMix * _rudder + _ailTrim; - _elevator += _elvTrim; - _rudder += _rdrTrim; - _throttle += _thrTrim; - } - void setMotors() { - // turn all motors off if below 0.1 throttle - if (fabs(_board->rc[ch_thrust]->getRadioPosition()) < 0.1) { - setAllRadioChannelsToNeutral(); - } else { - _board->rc[ch_roll]->setPosition(_aileron); - _board->rc[ch_yaw]->setPosition(_rudder); - _board->rc[ch_pitch]->setPosition(_elevator); - _board->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 { - 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 - }; - AP_Var_group _trimGroup; - AP_Uint8 _rdrAilMix; - bool _needsTrim; - AP_Float _ailTrim; - AP_Float _elvTrim; - AP_Float _rdrTrim; - AP_Float _thrTrim; - BlockPID pidBnkRll; // bank error to roll servo deflection - BlockPID pidSpdPit; // speed error to pitch command - BlockPID pidPitPit; // pitch error to pitch servo deflection - BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection - BlockPID pidHdgBnk; // heading error to bank command - BlockPID pidAltThr; // altitude error to throttle deflection - bool requireRadio; - float _aileron; - float _elevator; - float _rudder; - float _throttle; -}; - -} // namespace apo - -#endif /* CONTROLLERPLANE_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h deleted file mode 100644 index 7246ed6e22..0000000000 --- a/apo/ControllerQuad.h +++ /dev/null @@ -1,192 +0,0 @@ -/* - * ControllerQuad.h - * - * Created on: Jun 30, 2011 - * Author: jgoppert - */ - -#ifndef CONTROLLERQUAD_H_ -#define CONTROLLERQUAD_H_ - -#include "../APO/AP_Controller.h" -#include "../APO/AP_BatteryMonitor.h" -#include "../APO/AP_ArmingMechanism.h" - -namespace apo { - -class ControllerQuad: public AP_Controller { -public: - ControllerQuad(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board) : - AP_Controller(nav, guide, board, new AP_ArmingMechanism(board,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl), - 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), - pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1, - PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, - PID_ATT_LIM), - pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1, - PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D, - PID_YAWPOS_AWU, PID_YAWPOS_LIM), - 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), - pidSpeed(new AP_Var_group(k_pidSpeed, PSTR("SPD_")), 1, PID_SPD_P, - PID_SPD_I, PID_SPD_D, PID_SPD_AWU, PID_SPD_LIM, PID_SPD_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), - _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), - _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) { - _board->debug->println_P(PSTR("initializing quad controller")); - - /* - * allocate radio channels - * the order of the channels has to match the enumeration above - */ - _board->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), board->radio, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chRight, PSTR("RIGHT_"), board->radio, 0, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chLeft, PSTR("LEFT_"), board->radio, 1, 1100, - 1100, 1900, RC_MODE_OUT, false)); - - _board->rc.push_back( - new AP_RcChannel(k_chFront, PSTR("FRONT_"), board->radio, 2, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chBack, PSTR("BACK_"), board->radio, 3, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _board->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("ROLL_"), board->radio, 0, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("PITCH_"), board->radio, 1, 1100, - 1500, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THRUST_"), board->radio, 2, 1100, - 1100, 1900, RC_MODE_IN, false)); - _board->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), board->radio, 3, 1100, 1500, - 1900, RC_MODE_IN, false)); - } - -private: - // methods - void manualLoop(const float dt) { - setAllRadioChannelsManually(); - _cmdRoll = -0.5 * _board->rc[ch_roll]->getPosition(); - _cmdPitch = -0.5 * _board->rc[ch_pitch]->getPosition(); - _cmdYawRate = -1 * _board->rc[ch_yaw]->getPosition(); - _thrustMix = _board->rc[ch_thrust]->getPosition(); - autoAttitudeLoop(dt); - } - void autoLoop(const float dt) { - autoPositionLoop(dt); - autoAttitudeLoop(dt); - } - void autoPositionLoop(float dt) { - float cmdSpeed = pidSpeed.update(_guide->getGroundSpeedError(),dt); - float cmdDown = pidPD.update(_guide->getPDError(),-_nav->getVD(),dt); - - // tilt based control - float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt); - _cmdPitch = -cmdTilt*cos(_guide->getHeadingError()); - _cmdRoll = cmdTilt*sin(_guide->getHeadingError()); - - // add velocity based control - _cmdPitch -= cmdSpeed*cos(_nav->getRelativeCourseOverGround()); - _cmdRoll += cmdSpeed*sin(_nav->getRelativeCourseOverGround()); - - _cmdYawRate = pidYaw.update(_guide->getYawError(),-_nav->getYawRate(),dt); // always points to next waypoint - _thrustMix = THRUST_HOVER_OFFSET - cmdDown; - - // "thrust-trim-adjust" - if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393; - else _thrustMix /= cos(_cmdRoll); - - if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393; - else _thrustMix /= cos(_cmdPitch); - - // debug for position loop - //_board->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(), - -_nav->getRollRate(), dt); - _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(), - -_nav->getPitchRate(), dt); - _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); - } - void setMotors() { - // turn all motors off if below 0.1 throttle - if (fabs(_board->rc[ch_thrust]->getRadioPosition()) < 0.1) { - setAllRadioChannelsToNeutral(); - } else { - _board->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix); - _board->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix); - _board->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix); - _board->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix); - } - } - - void handleFailsafe() { - // turn off - setMode(MAV_MODE_LOCKED); - } - - // attributes - /** - * note that these are not the controller radio channel numbers, they are just - * unique keys so they can be reaccessed from the board 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_pidTilt, - k_pidSpeed, - k_pidPD, - k_pidRoll, - k_pidPitch, - k_pidYawRate, - k_pidYaw, - }; - BlockPIDDfb pidRoll, pidPitch, pidYaw; - BlockPID pidYawRate; - BlockPID pidTilt, pidSpeed; - BlockPIDDfb pidPD; - float _thrustMix, _pitchMix, _rollMix, _yawMix; - float _cmdRoll, _cmdPitch, _cmdYawRate; -}; - -} // namespace apo - -#endif /* CONTROLLERQUAD_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/apo/Makefile b/apo/Makefile deleted file mode 100644 index 1d9b6a022d..0000000000 --- a/apo/Makefile +++ /dev/null @@ -1 +0,0 @@ -include ../libraries/AP_Common/Arduino.mk diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h deleted file mode 100644 index 7ee78e75c5..0000000000 --- a/apo/PlaneEasystar.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * PlaneEasystar.h - * - * Created on: May 1, 2011 - * Author: jgoppert - */ - -#ifndef PLANEEASYSTAR_H_ -#define PLANEEASYSTAR_H_ - -using namespace apo; - -// vehicle options -static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; -static const MAV_TYPE vehicle = MAV_TYPE_FIXED_WING; -//static const apo::AP_Board::mode_e = apo::AP_Board::MODE_HIL_CNTL; -static const apo::AP_Board::mode_e = apo::AP_Board::MODE_LIVE; -static const uint8_t heartBeatTimeout = 0; - -// algorithm selection -#define CONTROLLER_CLASS ControllerPlane -#define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS Navigator_Dcm - -// hardware selection -//#define BOARD_TYPE Board_APM1 -//#define BOARD_TYPE Board_APM1_2560 -#define BOARD_TYPE Board_APM2 - -// loop rates -static const float loopRate = 150; // attitude nav -static const float loop0Rate = 50; // controller -static const float loop1Rate = 5; // pos nav/ gcs fast -static const float loop2Rate = 1; // gcs slow -static const float loop3Rate = 0.1; - -// gains -static const float rdrAilMix = 1.0; // since there are no ailerons - -// bank error to roll servo -static const float pidBnkRllP = -1; -static const float pidBnkRllI = 0.0; -static const float pidBnkRllD = 0.0; -static const float pidBnkRllAwu = 0.0; -static const float pidBnkRllLim = 1.0; -static const float pidBnkRllDFCut = 0.0; - -// pitch error to pitch servo -static const float pidPitPitP = -1; -static const float pidPitPitI = 0.0; -static const float pidPitPitD = 0.0; -static const float pidPitPitAwu = 0.0; -static const float pidPitPitLim = 1.0; -static const float pidPitPitDFCut = 0.0; - -// speed error to pitch command -static const float pidSpdPitP = 0.1; -static const float pidSpdPitI = 0.0; -static const float pidSpdPitD = 0.0; -static const float pidSpdPitAwu = 0.0; -static const float pidSpdPitLim = 1.0; -static const float pidSpdPitDFCut = 0.0; - -// yaw rate error to yaw servo -static const float pidYwrYawP = -0.1; -static const float pidYwrYawI = 0.0; -static const float pidYwrYawD = 0.0; -static const float pidYwrYawAwu = 0.0; -static const float pidYwrYawLim = 1.0; -static const float pidYwrYawDFCut = 0.0; - -// heading error to bank angle command -static const float pidHdgBnkP = 1.0; -static const float pidHdgBnkI = 0.0; -static const float pidHdgBnkD = 0.0; -static const float pidHdgBnkAwu = 0.0; -static const float pidHdgBnkLim = 0.5; -static const float pidHdgBnkDFCut = 0.0; - -// altitude error to throttle command -static const float pidAltThrP = .01; -static const float pidAltThrI = 0.0; -static const float pidAltThrD = 0.0; -static const float pidAltThrAwu = 0.0; -static const float pidAltThrLim = 1; -static const float pidAltThrDFCut = 0.0; - -// trim control positions (-1,1) -static const float ailTrim = 0.0; -static const float elvTrim = 0.0; -static const float rdrTrim = 0.0; -static const float thrTrim = 0.5; - -// guidance -static const float velCmd = 1; // m/s -static const float xt = 10; // cross track gain -static const float xtLim = 90; // cross track angle limit, deg - -#include "ControllerPlane.h" - -#endif /* PLANEEASYSTAR_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h deleted file mode 100644 index 7501367ed3..0000000000 --- a/apo/QuadArducopter.h +++ /dev/null @@ -1,109 +0,0 @@ -/* - * QuadArducopter.h - * - * Created on: May 1, 2011 - * Author: jgoppert - */ - -#ifndef QUADARDUCOPTER_H_ -#define QUADARDUCOPTER_H_ - -using namespace apo; - -// vehicle options -static const AP_Board::options_t options = AP_Board::opt_gps | AP_Board::opt_baro | AP_Board::opt_compass; -static const MAV_TYPE vehicle = MAV_QUADROTOR; -//static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_HIL_CNTL; -static const apo::AP_Board::mode_e boardMode = apo::AP_Board::MODE_LIVE; -static const uint8_t heartBeatTimeout = 0; - -// algorithm selection -#define CONTROLLER_CLASS ControllerQuad -#define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS Navigator_Dcm - -//// hardware selection -//#define BOARD_TYPE Board_APM1 -//#define BOARD_TYPE Board_APM1_2560 -#define BOARD_TYPE Board_APM2 - -// baud rates -// optional sensors -static const bool gpsEnabled = false; -static const bool baroEnabled = true; -static const bool compassEnabled = true; -static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; -// compass orientation: See AP_Compass_HMC5843.h for possible values - -// battery monitoring -static const bool batteryMonitorEnabled = false; -static const uint8_t batteryPin = 0; -static const float batteryVoltageDivRatio = 6; -static const float batteryMinVolt = 10.0; -static const float batteryMaxVolt = 12.4; - -static const bool rangeFinderFrontEnabled = false; -static const bool rangeFinderBackEnabled = false; -static const bool rangeFinderLeftEnabled = false; -static const bool rangeFinderRightEnabled = false; -static const bool rangeFinderUpEnabled = false; -static const bool rangeFinderDownEnabled = false; - -// loop rates -static const float loopRate = 250; // attitude nav -static const float loop0Rate = 50; // controller -static const float loop1Rate = 10; // pos nav/ gcs fast -static const float loop2Rate = 1; // gcs slow -static const float loop3Rate = 0.1; - -// position control loop -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_SPD_P = 1; -static const float PID_SPD_I = 0; -static const float PID_SPD_D = 0.1; -static const float PID_SPD_LIM = 0.04; // about 2 deg -static const float PID_SPD_AWU = 0.02; // about 1 deg -static const float PID_SPD_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_Z_DFCUT = 10; // cut derivative feedback at 10 hz - -// attitude control loop -static const float PID_ATT_P = 0.17; -static const float PID_ATT_I = 0.5; -static const float PID_ATT_D = 0.06; -static const float PID_ATT_LIM = 0.05; // 5 % motors -static const float PID_ATT_AWU = 0.005; // 0.5 % -static const float PID_ATT_DFCUT = 25; // cut derivative feedback at 25 hz -static const float PID_YAWPOS_P = 1; -static const float PID_YAWPOS_I = 0; -static const float PID_YAWPOS_D = 0.1; -static const float PID_YAWPOS_LIM = 1; // 1 rad/s -static const float PID_YAWPOS_AWU = 0; // 1 rad/s -static const float PID_YAWSPEED_P = 0.5; -static const float PID_YAWSPEED_I = 0; -static const float PID_YAWSPEED_D = 0; -static const float PID_YAWSPEED_LIM = .05; // 5 % motors -static const float PID_YAWSPEED_AWU = 0.0; -static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz -static const float THRUST_HOVER_OFFSET = 0.475; - -// guidance -static const float velCmd = 0.3; // m/s -static const float xt = 10; // cross track gain -static const float xtLim = 90; // cross track angle limit, deg - -#include "ControllerQuad.h" - -#endif /* QUADARDUCOPTER_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/apo/apo.pde b/apo/apo.pde deleted file mode 100644 index 2307566265..0000000000 --- a/apo/apo.pde +++ /dev/null @@ -1,24 +0,0 @@ -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Vehicle Configuration -#include "QuadArducopter.h" -//#include "PlaneEasystar.h" - -// ArduPilotOne Default Setup -#include "APO_DefaultSetup.h" diff --git a/libraries/APO/APO.cpp b/libraries/APO/APO.cpp deleted file mode 100644 index 2592d1140c..0000000000 --- a/libraries/APO/APO.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * APO.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "APO.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h deleted file mode 100644 index ec28083f12..0000000000 --- a/libraries/APO/APO.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * APO.h - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#ifndef APO_H_ -#define APO_H_ - -#include -#include "AP_Autopilot.h" -#include "AP_Guide.h" -#include "AP_Controller.h" -#include "AP_ControllerBlock.h" -#include "AP_Board.h" -#include "AP_MavlinkCommand.h" -#include "AP_Navigator.h" -#include "AP_RcChannel.h" -#include "AP_BatteryMonitor.h" -#include "AP_ArmingMechanism.h" -#include "AP_CommLink.h" -#include "AP_Var_keys.h" -#include "constants.h" - - -#include "Navigator_Dcm.h" - -#include "Board_APM1.h" -#include "Board_APM1_2560.h" -#include "Board_APM2.h" - -#endif /* APO_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h deleted file mode 100644 index 4cd2604d3b..0000000000 --- a/libraries/APO/APO_DefaultSetup.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef _APO_COMMON_H -#define _APO_COMMON_H - -FastSerialPort0(Serial); -FastSerialPort1(Serial1); -FastSerialPort2(Serial2); -FastSerialPort3(Serial3); - -/* - * Required Global Declarations - */ - -static apo::AP_Autopilot * autoPilot; - -void setup() { - - using namespace apo; - - // hardware abstraction layer - AP_Board * board = new BOARD_TYPE(boardMode, vehicle, options); - - /* - * Select guidance, navigation, control algorithms - */ - AP_Navigator * navigator = NULL; - if (board->getMode() == AP_Board::MODE_LIVE) { - navigator = new NAVIGATOR_CLASS(board,k_nav); - } else { - navigator = new AP_Navigator(board); - } - AP_Guide * guide = new GUIDE_CLASS(navigator, board, velCmd, xt, xtLim); - AP_Controller * controller = new CONTROLLER_CLASS(navigator,guide,board); - - /* - * Start the autopil/ot - */ - autoPilot = new apo::AP_Autopilot(navigator, guide, controller, board, - loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate); -} - -void loop() { - autoPilot->update(); -} - -#endif //_APO_COMMON_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp deleted file mode 100644 index 1d6702d489..0000000000 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * AP_ArmingMechanism.cpp - * - */ - -#include "AP_ArmingMechanism.h" -#include "AP_Controller.h" -#include "AP_RcChannel.h" -#include "../FastSerial/FastSerial.h" -#include "AP_Board.h" -#include "AP_CommLink.h" - -namespace apo { - -void AP_ArmingMechanism::update(const float dt) { - //_board->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_board->rc[_ch1]->getRadioPosition(), _board->rc[_ch2]->getRadioPosition()); - // arming - if ( (_controller->getState() != MAV_STATE_ACTIVE) && - (fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_board->rc[_ch2]->getRadioPosition() < _ch2Min) ) { - - // always start clock at 0 - if (_armingClock<0) _armingClock = 0; - - if (_armingClock++ >= 100) { - _controller->setMode(MAV_MODE_READY); - } else { - _board->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); - } - } - // disarming - else if ( (_controller->getState() == MAV_STATE_ACTIVE) && - (fabs(_board->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_board->rc[_ch2]->getRadioPosition() > _ch2Max) ) { - - // always start clock at 0 - if (_armingClock>0) _armingClock = 0; - - if (_armingClock-- <= -100) { - _controller->setMode(MAV_MODE_LOCKED); - } else { - _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); - } - } - // reset arming clock and report status - else if (_armingClock != 0) { - _armingClock = 0; - if (_controller->getState()==MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); - else if (_controller->getState()!=MAV_STATE_ACTIVE) _board->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); - } -} - -} // apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h deleted file mode 100644 index b09e94f8e4..0000000000 --- a/libraries/APO/AP_ArmingMechanism.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * AP_ArmingMechanism.h - * - */ - -#ifndef AP_ARMINGMECHANISM_H_ -#define AP_ARMINGMECHANISM_H_ - -#include -#include - -namespace apo { - -class AP_Board; -class AP_Controller; - -class AP_ArmingMechanism { - -public: - /** - * Constructor - * - * @param ch1: typically throttle channel - * @param ch2: typically yaw channel - * @param ch1Min: disarms/arms belows this - * @param ch2Min: disarms below this - * @param ch2Max: arms above this - */ - AP_ArmingMechanism(AP_Board * board, AP_Controller * controller, - uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, - float ch2Max) : _armingClock(0), _board(board), _controller(controller), - _ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { - } - - /** - * update - * - * arming: - * - * to arm: put stick to bottom right for 100 controller cycles - * (max yaw, min throttle) - * - * didn't use clock here in case of millis() roll over - * for long runs - * - * disarming: - * - * to disarm: put stick to bottom left for 100 controller cycles - * (min yaw, min throttle) - */ - void update(const float dt); - -private: - - AP_Board * _board; - AP_Controller * _controller; - int8_t _armingClock; - uint8_t _ch1; /// typically throttle channel - uint8_t _ch2; /// typically yaw channel - float _ch1Min; /// arms/disarms below this on ch1 - float _ch2Min; /// disarms below this on ch2 - float _ch2Max; /// arms above this on ch2 -}; - -} // namespace apo - -#endif /* AP_ARMINGMECHANISM */ - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp deleted file mode 100644 index bf1b8261e4..0000000000 --- a/libraries/APO/AP_Autopilot.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/* - * AP_Autopilot.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "../FastSerial/FastSerial.h" -#include "AP_Autopilot.h" -#include "../AP_GPS/AP_GPS.h" -#include "../APM_RC/APM_RC.h" -#include "AP_Board.h" -#include "AP_CommLink.h" -#include "AP_MavlinkCommand.h" -#include "AP_Navigator.h" -#include "AP_Controller.h" -#include "AP_Guide.h" -#include "AP_BatteryMonitor.h" - -namespace apo { - -class AP_Board; - -AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, - float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : - Loop(loopRate, callback, this), _navigator(navigator), _guide(guide), - _controller(controller), _board(board), - callbackCalls(0) { - - board->debug->printf_P(PSTR("initializing autopilot\n")); - board->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); - - /* - * Comm links - */ - board->gcs = new MavlinkComm(board->gcsPort, navigator, guide, controller, board, 3); - if (board->getMode() != AP_Board::MODE_LIVE) { - board->hil = new MavlinkComm(board->hilPort, navigator, guide, controller, board, 3); - } - board->gcsPort->printf_P(PSTR("gcs hello\n")); - - board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - - /* - * Calibration - */ - controller->setState(MAV_STATE_CALIBRATING); - board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - - if (navigator) navigator->calibrate(); - - /* - * Look for valid initial state - */ - while (_navigator) { - - // letc gcs known we are alive - board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - if (board->getMode() == AP_Board::MODE_LIVE) { - _navigator->updateSlow(0); - if (board->gps) { - if (board->gps->fix) { - break; - } else { - board->gps->update(); - board->gcs->sendText(SEVERITY_LOW, - PSTR("waiting for gps lock\n")); - board->debug->printf_P(PSTR("waiting for gps lock\n")); - } - } else { // no gps, can skip - break; - } - } else if (board->getMode() == AP_Board::MODE_HIL_CNTL) { // hil - board->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - board->hil->receive(); - if (_navigator->getTimeStamp() != 0) { - // give hil a chance to send some packets - for (int i = 0; i < 5; i++) { - board->debug->println_P(PSTR("reading initial hil packets")); - board->gcs->sendText(SEVERITY_LOW, PSTR("reading initial hil packets")); - delay(1000); - } - break; - } - board->debug->println_P(PSTR("waiting for hil packet")); - board->gcs->sendText(SEVERITY_LOW, PSTR("waiting for hil packets")); - } - delay(500); - } - - AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); - AP_MavlinkCommand::home.setLat(_navigator->getLat()); - AP_MavlinkCommand::home.setLon(_navigator->getLon()); - AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT); - AP_MavlinkCommand::home.save(); - _board->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), - AP_MavlinkCommand::home.getLat()*rad2Deg, - AP_MavlinkCommand::home.getLon()*rad2Deg, - AP_MavlinkCommand::home.getCommand()); - AP_MavlinkCommand::home.load(); - _board->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), - AP_MavlinkCommand::home.getLat()*rad2Deg, - 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 - */ - board->debug->println_P(PSTR("attaching loops")); - subLoops().push_back(new Loop(loop0Rate, callback0, this)); - subLoops().push_back(new Loop(loop1Rate, callback1, this)); - subLoops().push_back(new Loop(loop2Rate, callback2, this)); - subLoops().push_back(new Loop(loop3Rate, callback3, this)); - - board->debug->println_P(PSTR("running")); - board->gcs->sendText(SEVERITY_LOW, PSTR("running")); -} - -void AP_Autopilot::callback(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getBoard()->debug->println_P(PSTR("callback")); - - /* - * ahrs update - */ - apo->callbackCalls++; - if (apo->getNavigator()) - apo->getNavigator()->updateFast(apo->dt()); -} - -void AP_Autopilot::callback0(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getBoard()->debug->println_P(PSTR("callback 0")); - - /* - * hardware in the loop - */ - if (apo->getBoard()->hil && apo->getBoard()->getMode() != AP_Board::MODE_LIVE) { - apo->getBoard()->hil->receive(); - apo->getBoard()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); - } - - /* - * update control laws - */ - if (apo->getController()) { - //apo->getBoard()->debug->println_P(PSTR("updating controller")); - apo->getController()->update(apo->subLoops()[0]->dt()); - } - /* - char msg[50]; - sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); - apo->board()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); - */ -} - -void AP_Autopilot::callback1(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getBoard()->debug->println_P(PSTR("callback 1")); - - /* - * update guidance laws - */ - if (apo->getGuide()) - { - //apo->getBoard()->debug->println_P(PSTR("updating guide")); - apo->getGuide()->update(); - } - - /* - * slow navigation loop update - */ - if (apo->getNavigator()) { - apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt()); - } - - /* - * send telemetry - */ - if (apo->getBoard()->gcs) { - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); - } - - /* - * handle ground control station communication - */ - if (apo->getBoard()->gcs) { - // send messages - apo->getBoard()->gcs->requestCmds(); - apo->getBoard()->gcs->sendParameters(); - - // receive messages - apo->getBoard()->gcs->receive(); - } - - /* - * navigator debug - */ - /* - if (apo->navigator()) { - apo->getBoard()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), - apo->navigator()->getRoll()*rad2Deg, - apo->navigator()->getPitch()*rad2Deg, - apo->navigator()->getYaw()*rad2Deg); - apo->getBoard()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), - apo->navigator()->getLat()*rad2Deg, - apo->navigator()->getLon()*rad2Deg, - apo->navigator()->getAlt()); - } - */ -} - -void AP_Autopilot::callback2(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getBoard()->debug->println_P(PSTR("callback 2")); - - /* - * send telemetry - */ - if (apo->getBoard()->gcs) { - // send messages - //apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT); - //apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION); - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); - } - - /* - * update battery monitor - */ - if (apo->getBoard()->batteryMonitor) apo->getBoard()->batteryMonitor->update(); - - /* - * send heartbeat - */ - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - - /* - * load/loop rate/ram debug - */ - apo->getBoard()->load = apo->load(); - apo->getBoard()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls); - apo->callbackCalls = 0; - apo->getBoard()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), - apo->load(),1.0/apo->dt(),freeMemory()); - apo->getBoard()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - - /* - * adc debug - */ - //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), - //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), - //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), - //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); -} - -void AP_Autopilot::callback3(void * data) { - //AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getBoard()->debug->println_P(PSTR("callback 3")); -} - -} // apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h deleted file mode 100644 index 71196f6ff0..0000000000 --- a/libraries/APO/AP_Autopilot.h +++ /dev/null @@ -1,123 +0,0 @@ -/* - * AP_Autopilot.h - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#ifndef AP_AUTOPILOT_H_ -#define AP_AUTOPILOT_H_ - -#include "AP_Loop.h" - -/** - * ArduPilotOne namespace to protect variables - * from overlap with avr and libraries etc. - * ArduPilotOne does not use any global - * variables. - */ - -namespace apo { - -// enumerations -// forward declarations -class AP_Navigator; -class AP_Guide; -class AP_Controller; -class AP_Board; - -/** - * This class encapsulates the entire autopilot system - * The constructor takes guide, navigator, and controller - * as well as the hardware abstraction layer. - * - * It inherits from loop to manage - * the sub-loops and sets the overall - * frequency for the autopilot. - * - - */ -class AP_Autopilot: public Loop { -public: - /** - * Default constructor - */ - AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, - float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); - - /** - * Accessors - */ - AP_Navigator * getNavigator() { - return _navigator; - } - AP_Guide * getGuide() { - return _guide; - } - AP_Controller * getController() { - return _controller; - } - AP_Board * getBoard() { - return _board; - } - - /** - * Loop Monitoring - */ - uint32_t callbackCalls; - -private: - - /** - * Loop Callbacks (fastest) - * - inertial navigation - * @param data A void pointer used to pass the apo class - * so that the apo public interface may be accessed. - */ - static void callback(void * data); - - /** - * Loop 0 Callbacks - * - control - * - compass reading - * @see callback - */ - static void callback0(void * data); - - /** - * Loop 1 Callbacks - * - gps sensor fusion - * - compass sensor fusion - * @see callback - */ - static void callback1(void * data); - - /** - * Loop 2 Callbacks - * - slow messages - * @see callback - */ - static void callback2(void * data); - - /** - * Loop 3 Callbacks - * - super slow messages - * - log writing - * @see callback - */ - static void callback3(void * data); - - /** - * Components - */ - AP_Navigator * _navigator; - AP_Guide * _guide; - AP_Controller * _controller; - AP_Board * _board; -}; - -} // namespace apo - -#endif /* AP_AUTOPILOT_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_BatteryMonitor.cpp b/libraries/APO/AP_BatteryMonitor.cpp deleted file mode 100644 index 835f3072d2..0000000000 --- a/libraries/APO/AP_BatteryMonitor.cpp +++ /dev/null @@ -1,12 +0,0 @@ -/* - * AP_BatteryMonitor.cpp - * - */ - -#include "AP_BatteryMonitor.h" - -namespace apo { - -} // apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_BatteryMonitor.h b/libraries/APO/AP_BatteryMonitor.h deleted file mode 100644 index 359320a368..0000000000 --- a/libraries/APO/AP_BatteryMonitor.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * AP_BatteryMonitor.h - * - */ - -#ifndef AP_BATTERYMONITOR_H_ -#define AP_BATTERYMONITOR_H_ - -#include -#include - -namespace apo { - -class AP_BatteryMonitor { -public: - AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) : - _pin(pin), _voltageDivRatio(voltageDivRatio), - _minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) { - } - - void update() { - // low pass filter on voltage - _voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1; - } - - /** - * Accessors - */ - float getVoltage() { - return _voltage; - } - - uint8_t getPercentage() { - float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt); - if (norm < 0) norm = 0; - else if (norm > 1) norm = 1; - return 100*norm; - } - -private: - - uint8_t _pin; - float _voltageDivRatio; - float _voltage; - float _minVolt; - float _maxVolt; -}; - -} // namespace apo - -#endif /* AP_BATTERYMONITOR_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Board.cpp b/libraries/APO/AP_Board.cpp deleted file mode 100644 index ffb430b293..0000000000 --- a/libraries/APO/AP_Board.cpp +++ /dev/null @@ -1,14 +0,0 @@ -/* - * AP_Board.cpp - * - * Created on: Apr 4, 2011 - * - */ - -#include "AP_Board.h" - -namespace apo { - -} // namespace apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Board.h b/libraries/APO/AP_Board.h deleted file mode 100644 index af88062855..0000000000 --- a/libraries/APO/AP_Board.h +++ /dev/null @@ -1,139 +0,0 @@ -/* - * AP_Board.h - * - * Created on: Apr 4, 2011 - * - */ - -#ifndef AP_BOARD_H_ -#define AP_BOARD_H_ - -#include "../AP_Common/AP_Vector.h" -#include "../GCS_MAVLink/GCS_MAVLink.h" - -class AP_ADC; -class IMU; -class GPS; -class APM_BMP085_Class; -class Compass; -class BetterStream; -class RangeFinder; -class FastSerial; -class AP_IMU_INS; -class AP_InertialSensor; -class APM_RC_Class; -class AP_TimerProcess; -class Arduino_Mega_ISR_Registry; -class DataFlash_Class; - -namespace apo { - -class AP_RcChannel; -class AP_CommLink; -class AP_BatteryMonitor; - -class AP_Board { - -public: - - typedef uint32_t options_t; - options_t _options; - - // enumerations - enum mode_e { - MODE_LIVE, MODE_HIL_CNTL, - /*MODE_HIL_NAV*/ - }; - - - enum options_e { - opt_gps = 0<<1, - opt_baro = 1<<1, - opt_compass = 2<<1, - opt_batteryMonitor = 3<<1, - opt_rangeFinderFront = 4<<1, - opt_rangeFinderBack = 5<<1, - opt_rangeFinderLeft = 6<<1, - opt_rangeFinderRight = 7<<1, - opt_rangeFinderUp = 8<<1, - opt_rangeFinderDown = 9<<1, - }; - - // default ctors on pointers called on pointers here, this - // allows NULL to be used as a boolean for if the device was - // initialized - AP_Board(mode_e mode, MAV_TYPE vehicle, options_t options): _mode(mode), _vehicle(vehicle), _options(options) { - } - - /** - * Sensors - */ - AP_ADC * adc; - GPS * gps; - APM_BMP085_Class * baro; - Compass * compass; - Vector rangeFinders; - AP_BatteryMonitor * batteryMonitor; - AP_IMU_INS * imu; - AP_InertialSensor * ins; - - /** - * Scheduler - */ - AP_TimerProcess * scheduler; - Arduino_Mega_ISR_Registry * isr_registry; - - /** - * Actuators - */ - APM_RC_Class * radio; - - /** - * Radio Channels - */ - Vector rc; - - /** - * Communication Channels - */ - AP_CommLink * gcs; - AP_CommLink * hil; - FastSerial * debug; - FastSerial * gcsPort; - FastSerial * hilPort; - - /** - * data - */ - DataFlash_Class * dataFlash; - uint8_t load; - - /** - * settings - */ - uint8_t slideSwitchPin; - uint8_t pushButtonPin; - uint8_t aLedPin; - uint8_t bLedPin; - uint8_t cLedPin; - uint16_t eepromMaxAddr; - - // accessors - mode_e getMode() { - return _mode; - } - MAV_TYPE getVehicle() { - return _vehicle; - } - -private: - - // enumerations - mode_e _mode; - MAV_TYPE _vehicle; -}; - -} // namespace apo - -#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp deleted file mode 100644 index 30d473e9eb..0000000000 --- a/libraries/APO/AP_CommLink.cpp +++ /dev/null @@ -1,800 +0,0 @@ -/* - * AP_CommLink.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "../FastSerial/FastSerial.h" -#include "AP_CommLink.h" -#include "AP_Navigator.h" -#include "AP_Guide.h" -#include "AP_Controller.h" -#include "AP_MavlinkCommand.h" -#include "AP_Board.h" -#include "AP_RcChannel.h" -#include "../AP_GPS/AP_GPS.h" -#include "../AP_Math/AP_Math.h" -#include "../AP_IMU/AP_IMU.h" -#include "../AP_Compass/AP_Compass.h" -#include "AP_BatteryMonitor.h" - -namespace apo { - -uint8_t MavlinkComm::_nChannels = 0; -uint8_t MavlinkComm::_paramNameLengthMax = 13; - -AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, - const uint16_t heartBeatTimeout) : - _link(link), _navigator(navigator), _guide(guide), - _controller(controller), _board(board), _heartBeatTimeout(heartBeatTimeout), _lastHeartBeat(0) { -} - -MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, - const uint16_t heartBeatTimeout) : - AP_CommLink(link, nav, guide, controller, board,heartBeatTimeout), - - // options - _useRelativeAlt(true), - - // commands - _sendingCmds(false), _receivingCmds(false), - _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), - _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), - _cmdMax(30), _cmdNumberRequested(0), - - // parameters - _parameterCount(0), _queuedParameter(NULL), - _queuedParameterIndex(0) { - - switch (_nChannels) { - case 0: - mavlink_comm_0_port = link; - _channel = MAVLINK_COMM_0; - _nChannels++; - break; - case 1: - mavlink_comm_1_port = link; - _channel = MAVLINK_COMM_1; - _nChannels++; - break; - default: - // signal that number of channels exceeded - _channel = MAVLINK_COMM_3; - break; - } -} - -void MavlinkComm::send() { - // if number of channels exceeded return - if (_channel == MAVLINK_COMM_3) - return; -} - -void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { - //_board->debug->printf_P(PSTR("send message\n")); - - // if number of channels exceeded return - if (_channel == MAVLINK_COMM_3) - return; - - uint64_t timeStamp = micros(); - - switch (id) { - - case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_msg_heartbeat_send(_channel, _board->getVehicle(), - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - } - - case MAVLINK_MSG_ID_ATTITUDE: { - mavlink_msg_attitude_send(_channel, timeStamp, - _navigator->getRoll(), _navigator->getPitch(), - _navigator->getYaw(), _navigator->getRollRate(), - _navigator->getPitchRate(), _navigator->getYawRate()); - break; - } - - case MAVLINK_MSG_ID_GLOBAL_POSITION: { - mavlink_msg_global_position_send(_channel, timeStamp, - _navigator->getLat() * rad2Deg, - _navigator->getLon() * rad2Deg, _navigator->getAlt(), - _navigator->getVN(), _navigator->getVE(), - _navigator->getVD()); - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: { - mavlink_msg_local_position_send(_channel, timeStamp, - _navigator->getPN(),_navigator->getPE(), _navigator->getPD(), - _navigator->getVN(), _navigator->getVE(), _navigator->getVD()); - break; - } - - case MAVLINK_MSG_ID_GPS_RAW: { - mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(), - _board->gps->latitude/1.0e7, - _board->gps->longitude/1.0e7, _board->gps->altitude/100.0, 0, 0, - _board->gps->ground_speed/100.0, - _board->gps->ground_course/10.0); - break; - } - - case MAVLINK_MSG_ID_GPS_RAW_INT: { - mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(), - _board->gps->latitude, - _board->gps->longitude, _board->gps->altitude*10.0, 0, 0, - _board->gps->ground_speed/100.0, - _board->gps->ground_course/10.0); - break; - } - - case MAVLINK_MSG_ID_SCALED_IMU: { - int16_t xmag, ymag, zmag; - xmag = ymag = zmag = 0; - if (_board->compass) { - // XXX THIS IS NOT SCALED - xmag = _board->compass->mag_x; - ymag = _board->compass->mag_y; - zmag = _board->compass->mag_z; - } - mavlink_msg_scaled_imu_send(_channel, timeStamp, - _navigator->getXAccel()*1e3, - _navigator->getYAccel()*1e3, - _navigator->getZAccel()*1e3, - _navigator->getRollRate()*1e3, - _navigator->getPitchRate()*1e3, - _navigator->getYawRate()*1e3, - xmag, ymag, zmag); - break; - } - - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { - int16_t ch[8]; - for (int i = 0; i < 8; i++) - ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) { - ch[i] = 10000 * _board->rc[i]->getPosition(); - //_board->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); - } - mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], - ch[3], ch[4], ch[5], ch[6], ch[7], 255); - break; - } - - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { - int16_t ch[8]; - for (int i = 0; i < 8; i++) - ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) - ch[i] = _board->rc[i]->getPwm(); - mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], - ch[3], ch[4], ch[5], ch[6], ch[7], 255); - break; - } - - case MAVLINK_MSG_ID_SYS_STATUS: { - - uint16_t batteryVoltage = 0; // (milli volts) - uint16_t batteryPercentage = 1000; // times 10 - if (_board->batteryMonitor) { - batteryPercentage = _board->batteryMonitor->getPercentage()*10; - batteryVoltage = _board->batteryMonitor->getVoltage()*1000; - } - mavlink_msg_sys_status_send(_channel, _controller->getMode(), - _guide->getMode(), _controller->getState(), _board->load * 10, - batteryVoltage, batteryPercentage, _packetDrops); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: { - sendText(SEVERITY_LOW, PSTR("waypoint ack")); - //mavlink_waypoint_ack_t packet; - uint8_t type = 0; // ok (0), error(1) - mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, - _cmdDestCompId, type); - - // turn off waypoint send - _receivingCmds = false; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { - mavlink_msg_waypoint_current_send(_channel, - _guide->getCurrentIndex()); - break; - } - - default: { - char msg[50]; - sprintf(msg, "autopilot sending unknown command with id: %d", id); - sendText(SEVERITY_HIGH, msg); - } - - } // switch -} // send message - -void MavlinkComm::receive() { - //_board->debug->printf_P(PSTR("receive\n")); - // if number of channels exceeded return - // - if (_channel == MAVLINK_COMM_3) - return; - - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - status.packet_rx_drop_count = 0; - - // process received bytes - while (comm_get_available(_channel)) { - uint8_t c = comm_receive_ch(_channel); - - // Try to get a new message - if (mavlink_parse_char(_channel, c, &msg, &status)) - _handleMessage(&msg); - } - - // Update packet drops counter - _packetDrops += status.packet_rx_drop_count; -} - -void MavlinkComm::sendText(uint8_t severity, const char *str) { - mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); -} - -void MavlinkComm::sendText(uint8_t severity, const prog_char_t *str) { - mavlink_statustext_t m; - uint8_t i; - for (i = 0; i < sizeof(m.text); i++) { - m.text[i] = pgm_read_byte((const prog_char *) (str++)); - } - if (i < sizeof(m.text)) - m.text[i] = 0; - sendText(severity, (const char *) m.text); -} - -void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { -} - -/** - * sends parameters one at a time - */ -void MavlinkComm::sendParameters() { - //_board->debug->printf_P(PSTR("send parameters\n")); - // Check to see if we are sending parameters - while (NULL != _queuedParameter) { - AP_Var *vp; - float value; - - // copy the current parameter and prepare to move to the next - vp = _queuedParameter; - _queuedParameter = _queuedParameter->next(); - - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(); - if (!isnan(value)) { - - char paramName[_paramNameLengthMax]; - vp->copy_name(paramName, sizeof(paramName)); - - mavlink_msg_param_value_send(_channel, (int8_t*) paramName, - value, _countParameters(), _queuedParameterIndex); - - _queuedParameterIndex++; - break; - } - } - -} - -/** - * request commands one at a time - */ -void MavlinkComm::requestCmds() { - //_board->debug->printf_P(PSTR("requesting commands\n")); - // request cmds one by one - if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { - mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, - _cmdDestCompId, _cmdRequestIndex); - } -} - -void MavlinkComm::_handleMessage(mavlink_message_t * msg) { - - uint32_t timeStamp = micros(); - - switch (msg->msgid) { - _board->debug->printf_P(PSTR("message received: %d"), msg->msgid); - - case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_heartbeat_t packet; - mavlink_msg_heartbeat_decode(msg, &packet); - _lastHeartBeat = micros(); - break; - } - - case MAVLINK_MSG_ID_GPS_RAW: { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - _navigator->setTimeStamp(timeStamp); - _navigator->setLat(packet.lat * deg2Rad); - _navigator->setLon(packet.lon * deg2Rad); - _navigator->setAlt(packet.alt); - _navigator->setYaw(packet.hdg * deg2Rad); - _navigator->setGroundSpeed(packet.v); - _navigator->setAirSpeed(packet.v); - //_board->debug->printf_P(PSTR("received hil gps raw packet\n")); - /* - _board->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), - packet.lat, - packet.lon, - packet.alt); - */ - break; - } - - case MAVLINK_MSG_ID_HIL_STATE: { - // decode - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(msg, &packet); - _navigator->setTimeStamp(timeStamp); - _navigator->setRoll(packet.roll); - _navigator->setPitch(packet.pitch); - _navigator->setYaw(packet.yaw); - _navigator->setRollRate(packet.rollspeed); - _navigator->setPitchRate(packet.pitchspeed); - _navigator->setYawRate(packet.yawspeed); - _navigator->setVN(packet.vx/ 1e2); - _navigator->setVE(packet.vy/ 1e2); - _navigator->setVD(packet.vz/ 1e2); - _navigator->setLat_degInt(packet.lat); - _navigator->setLon_degInt(packet.lon); - _navigator->setAlt(packet.alt / 1e3); - _navigator->setXAccel(packet.xacc/ 1e3); - _navigator->setYAccel(packet.xacc/ 1e3); - _navigator->setZAccel(packet.xacc/ 1e3); - break; - } - - case MAVLINK_MSG_ID_ATTITUDE: { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set dcm hil sensor - _navigator->setTimeStamp(timeStamp); - _navigator->setRoll(packet.roll); - _navigator->setPitch(packet.pitch); - _navigator->setYaw(packet.yaw); - _navigator->setRollRate(packet.rollspeed); - _navigator->setPitchRate(packet.pitchspeed); - _navigator->setYawRate(packet.yawspeed); - //_board->debug->printf_P(PSTR("received hil attitude packet\n")); - break; - } - - case MAVLINK_MSG_ID_ACTION: { - // decode - mavlink_action_t packet; - mavlink_msg_action_decode(msg, &packet); - if (_checkTarget(packet.target, packet.target_component)) - break; - - // do action - sendText(SEVERITY_LOW, PSTR("action received")); - switch (packet.action) { - - case MAV_ACTION_STORAGE_READ: - AP_Var::load_all(); - break; - - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_all(); - break; - - case MAV_ACTION_MOTORS_START: - _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: - _controller->setMode(MAV_MODE_LOCKED); - _navigator->calibrate(); - break; - - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - _controller->setMode(MAV_MODE_LOCKED); - break; - - case MAV_ACTION_LAUNCH: - case MAV_ACTION_TAKEOFF: - _guide->setMode(MAV_NAV_LIFTOFF); - break; - - case MAV_ACTION_LAND: - _guide->setCurrentIndex(0); - _guide->setMode(MAV_NAV_LANDING); - break; - - case MAV_ACTION_EMCY_LAND: - _guide->setMode(MAV_NAV_LANDING); - break; - - case MAV_ACTION_LOITER: - case MAV_ACTION_HALT: - _guide->setMode(MAV_NAV_LOITER); - break; - - case MAV_ACTION_SET_AUTO: - _controller->setMode(MAV_MODE_AUTO); - break; - - case MAV_ACTION_SET_MANUAL: - _controller->setMode(MAV_MODE_MANUAL); - break; - - case MAV_ACTION_RETURN: - _guide->setMode(MAV_NAV_RETURNING); - break; - - case MAV_ACTION_NAVIGATE: - case MAV_ACTION_CONTINUE: - _guide->setMode(MAV_NAV_WAYPOINT); - break; - - case MAV_ACTION_CALIBRATE_RC: - case MAV_ACTION_REBOOT: - case MAV_ACTION_REC_START: - case MAV_ACTION_REC_PAUSE: - case MAV_ACTION_REC_STOP: - sendText(SEVERITY_LOW, PSTR("action not implemented")); - break; - default: - sendText(SEVERITY_LOW, PSTR("unknown action")); - break; - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { - sendText(SEVERITY_LOW, PSTR("waypoint request list")); - - // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // Start sending waypoints - mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, - _guide->getNumberOfCommands()); - - _cmdTimeLastSent = millis(); - _cmdTimeLastReceived = millis(); - _sendingCmds = true; - _receivingCmds = false; - _cmdDestSysId = msg->sysid; - _cmdDestCompId = msg->compid; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { - sendText(SEVERITY_LOW, PSTR("waypoint request")); - - // Check if sending waypiont - if (!_sendingCmds) - break; - - // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - _board->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); - AP_MavlinkCommand cmd(packet.seq); - - mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); - mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, - wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, - wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, - wp.z); - - // update last waypoint comm stamp - _cmdTimeLastSent = millis(); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: { - sendText(SEVERITY_LOW, PSTR("waypoint ack")); - - // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // check for error - //uint8_t type = packet.type; // ok (0), error(1) - - // turn off waypoint send - _sendingCmds = false; - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - sendText(SEVERITY_LOW, PSTR("param request list")); - - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // Start sending parameters - next call to ::update will kick the first one out - - _queuedParameter = AP_Var::first(); - _queuedParameterIndex = 0; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { - sendText(SEVERITY_LOW, PSTR("waypoint clear all")); - - // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // clear all waypoints - uint8_t type = 0; // ok (0), error(1) - _guide->setNumberOfCommands(1); - _guide->setCurrentIndex(0); - - // send acknowledgement 3 times to makes sure it is received - for (int i = 0; i < 3; i++) - mavlink_msg_waypoint_ack_send(_channel, msg->sysid, - msg->compid, type); - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { - sendText(SEVERITY_LOW, PSTR("waypoint set current")); - - // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); - Serial.print("Packet Sequence:"); - Serial.println(packet.seq); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // set current waypoint - Serial.print("Current Index:"); - Serial.println(_guide->getCurrentIndex()); - Serial.flush(); - _guide->setCurrentIndex(packet.seq); - mavlink_msg_waypoint_current_send(_channel, - _guide->getCurrentIndex()); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: { - sendText(SEVERITY_LOW, PSTR("waypoint count")); - - // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // start waypoint receiving - if (packet.count > _cmdMax) { - packet.count = _cmdMax; - } - _cmdNumberRequested = packet.count; - _cmdTimeLastReceived = millis(); - _receivingCmds = true; - _sendingCmds = false; - _cmdRequestIndex = 0; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: { - sendText(SEVERITY_LOW, PSTR("waypoint")); - - // Check if receiving waypiont - if (!_receivingCmds) { - //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); - break; - } - - // decode - mavlink_waypoint_t packet; - mavlink_msg_waypoint_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // check if this is the requested waypoint - if (packet.seq != _cmdRequestIndex) { - char warningMsg[50]; - sprintf(warningMsg, - "waypoint request out of sequence: (packet) %d / %d (ap)", - packet.seq, _cmdRequestIndex); - sendText(SEVERITY_HIGH, warningMsg); - break; - } - - _board->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), - packet.x, - packet.y, - packet.z); - - // store waypoint - AP_MavlinkCommand command(packet); - //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); - _cmdRequestIndex++; - if (_cmdRequestIndex == _cmdNumberRequested) { - 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; - } - _cmdTimeLastReceived = millis(); - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: { - sendText(SEVERITY_LOW, PSTR("param set")); - AP_Var *vp; - AP_Meta_class::Type_id var_type; - - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // set parameter - - char key[_paramNameLengthMax + 1]; - strncpy(key, (char *) packet.param_id, _paramNameLengthMax); - key[_paramNameLengthMax] = 0; - - // find the requested parameter - vp = AP_Var::find(key); - if ((NULL != vp) && // exists - !isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - // add a small amount before casting parameter values - // from float to integer to avoid truncating to the - // next lower integer value. - const float rounding_addition = 0.01; - - // fetch the variable type ID - var_type = vp->meta_type_id(); - - // handle variables with standard type IDs - if (var_type == AP_Var::k_typeid_float) { - ((AP_Float *) vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_float16) { - ((AP_Float16 *) vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_int32) { - ((AP_Int32 *) vp)->set_and_save( - packet.param_value + rounding_addition); - - } else if (var_type == AP_Var::k_typeid_int16) { - ((AP_Int16 *) vp)->set_and_save( - packet.param_value + rounding_addition); - - } else if (var_type == AP_Var::k_typeid_int8) { - ((AP_Int8 *) vp)->set_and_save( - packet.param_value + rounding_addition); - } else { - // we don't support mavlink set on this parameter - break; - } - - // Report back the new value if we accepted the change - // we send the value we actually set, which could be - // different from the value sent, in case someone sent - // a fractional value to an integer type - mavlink_msg_param_value_send(_channel, (int8_t *) key, - vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... - } - - break; - } // end case - - - } -} - -uint16_t MavlinkComm::_countParameters() { - // if we haven't cached the parameter count yet... - if (0 == _parameterCount) { - AP_Var *vp; - - vp = AP_Var::first(); - do { - // if a parameter responds to cast_to_float then we are going to be able to report it - if (!isnan(vp->cast_to_float())) { - _parameterCount++; - } - } while (NULL != (vp = vp->next())); - } - return _parameterCount; -} - -AP_Var * _findParameter(uint16_t index) { - AP_Var *vp; - - vp = AP_Var::first(); - while (NULL != vp) { - - // if the parameter is reportable - if (!(isnan(vp->cast_to_float()))) { - // if we have counted down to the index we want - if (0 == index) { - // return the parameter - return vp; - } - // count off this parameter, as it is reportable but not - // the one we want - index--; - } - // and move to the next parameter - vp = vp->next(); - } - return NULL; -} - -// check the target -uint8_t MavlinkComm::_checkTarget(uint8_t sysid, uint8_t compid) { - /* - char msg[50]; - sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, - mavlink_system.sysid, compid, mavlink_system.compid); - sendText(SEVERITY_LOW, msg); - */ - if (sysid != mavlink_system.sysid) { - //sendText(SEVERITY_LOW, PSTR("system id mismatch")); - return 1; - - } else if (compid != mavlink_system.compid) { - //sendText(SEVERITY_LOW, PSTR("component id mismatch")); - return 0; // XXX currently not receiving correct compid from gcs - - } else { - return 0; // no error - } -} - -} // apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h deleted file mode 100644 index 0cad2ae62b..0000000000 --- a/libraries/APO/AP_CommLink.h +++ /dev/null @@ -1,141 +0,0 @@ -/* - * AP_CommLink.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_CommLink_H -#define AP_CommLink_H - -#include -#include "../AP_Common/AP_Common.h" -#include "../AP_Common/AP_Vector.h" -#include "../GCS_MAVLink/GCS_MAVLink.h" - -class FastSerial; - -namespace apo { - -class AP_Controller; -class AP_Navigator; -class AP_Guide; -class AP_Board; - -enum { - SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH -}; - -// forward declarations -//class ArduPilotOne; -//class AP_Controller; - -/// CommLink class -class AP_CommLink { -public: - - AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, const uint16_t heartBeatTimeout); - virtual void send() = 0; - virtual void receive() = 0; - virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; - virtual void sendText(uint8_t severity, const char *str) = 0; - virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; - virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; - virtual void sendParameters() = 0; - virtual void requestCmds() = 0; - - /// check if heartbeat is lost - bool heartBeatLost() { - if (_heartBeatTimeout == 0) - return false; - else - return ((micros() - _lastHeartBeat) / 1e6) > _heartBeatTimeout; - } - -protected: - FastSerial * _link; - AP_Navigator * _navigator; - AP_Guide * _guide; - AP_Controller * _controller; - AP_Board * _board; - uint16_t _heartBeatTimeout; /// vehicle heartbeat timeout, s - uint32_t _lastHeartBeat; /// time of last heartbeat, s -}; - -class MavlinkComm: public AP_CommLink { -public: - MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_Board * board, uint16_t heartBeatTimeout); - - virtual void send(); - void sendMessage(uint8_t id, uint32_t param = 0); - virtual void receive(); - void sendText(uint8_t severity, const char *str); - void sendText(uint8_t severity, const prog_char_t *str); - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); - - /** - * sends parameters one at a time - */ - void sendParameters(); - - /** - * request commands one at a time - */ - void requestCmds(); - -private: - - // options - bool _useRelativeAlt; - - // commands - bool _sendingCmds; - bool _receivingCmds; - uint16_t _cmdTimeLastSent; - uint16_t _cmdTimeLastReceived; - uint16_t _cmdDestSysId; - uint16_t _cmdDestCompId; - uint16_t _cmdRequestIndex; - uint16_t _cmdNumberRequested; - uint16_t _cmdMax; - Vector _cmdList; - - // parameters - static uint8_t _paramNameLengthMax; - uint16_t _parameterCount; - AP_Var * _queuedParameter; - uint16_t _queuedParameterIndex; - - // channel - mavlink_channel_t _channel; - uint16_t _packetDrops; - static uint8_t _nChannels; - - void _handleMessage(mavlink_message_t * msg); - - uint16_t _countParameters(); - - AP_Var * _findParameter(uint16_t index); - - // check the target - uint8_t _checkTarget(uint8_t sysid, uint8_t compid); - -}; - -} // namespace apo - -#endif // AP_CommLink_H -// vim:ts=4:sw=4:tw=78:expandtab diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp deleted file mode 100644 index 40219af450..0000000000 --- a/libraries/APO/AP_Controller.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* - * AP_Controller.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "../FastSerial/FastSerial.h" -#include "AP_ArmingMechanism.h" -#include "AP_BatteryMonitor.h" -#include "AP_Board.h" -#include "AP_RcChannel.h" -#include "../GCS_MAVLink/include/mavlink_types.h" -#include "constants.h" -#include "AP_CommLink.h" -#include "AP_Controller.h" - -namespace apo { - -AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board, AP_ArmingMechanism * armingMechanism, - const uint8_t chMode, const uint16_t key, const prog_char_t * name) : - _nav(nav), _guide(guide), _board(board), _armingMechanism(armingMechanism), - _group(key, name ? : PSTR("CNTRL_")), - _chMode(chMode), - _mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) { - setAllRadioChannelsToNeutral(); -} - -void AP_Controller::setAllRadioChannelsToNeutral() { - for (uint8_t i = 0; i < _board->rc.getSize(); i++) { - _board->rc[i]->setPosition(0.0); - } -} - -void AP_Controller::setAllRadioChannelsManually() { - //_board->debug->printf_P(PSTR("\tsize: %d\n"),_board->rc.getSize()); - for (uint8_t i = 0; i < _board->rc.getSize(); i++) { - _board->rc[i]->setUsingRadio(); - //_board->debug->printf_P(PSTR("\trc %d: %f\n"),i,_board->rc[i]->getPosition()); - } -} - -void AP_Controller::update(const float dt) { - if (_armingMechanism) _armingMechanism->update(dt); - // 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); - _board->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); - _board->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); - } - - // handle emergencies - // - // check for heartbeat from gcs, if not found go to failsafe - if (_board->gcs->heartBeatLost()) { - setMode(MAV_MODE_FAILSAFE); - _board->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); - - // if battery less than 5%, go to failsafe - } else if (_board->batteryMonitor && _board->batteryMonitor->getPercentage() < 5) { - setMode(MAV_MODE_FAILSAFE); - _board->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); - } - - // if in auto mode and manual switch set, change to manual - if (_board->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL); - else setMode(MAV_MODE_AUTO); - - // handle all possible modes - if (getMode()==MAV_MODE_MANUAL) { - manualLoop(dt); - } else if (getMode()==MAV_MODE_AUTO) { - autoLoop(dt); - } else if (getMode()==MAV_MODE_FAILSAFE) { - handleFailsafe(); - } else { - _board->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); - setMode(MAV_MODE_FAILSAFE); - } - } - - // this sends commands to motors - if(getState()==MAV_STATE_ACTIVE) { - digitalWrite(_board->aLedPin, HIGH); - setMotors(); - } else { - digitalWrite(_board->aLedPin, LOW); - setAllRadioChannelsToNeutral(); - } -} - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h deleted file mode 100644 index e35bf618eb..0000000000 --- a/libraries/APO/AP_Controller.h +++ /dev/null @@ -1,148 +0,0 @@ -/* - * AP_Controller.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_Controller_H -#define AP_Controller_H - -// inclusions -#include "../AP_Common/AP_Common.h" -#include "../AP_Common/AP_Var.h" -#include -#include -#include "../GCS_MAVLink/GCS_MAVLink.h" - -namespace apo { - -// forward declarations within apo -class AP_Board; -class AP_Guide; -class AP_Navigator; -class Menu; -class AP_ArmingMechanism; - -/// -// The control system class. -// Given where the vehicle wants to go and where it is, -// this class is responsible for sending commands to the -// motors. It is also responsible for monitoring manual -// input. -class AP_Controller { -public: - /// - // The controller constructor. - // Creates the control system. - // @nav the navigation system - // @guide the guidance system - // @board the hardware abstraction layer - // @armingMechanism the device that controls arming/ disarming - // @chMode the channel that the mode switch is on - // @key the unique key for the control system saved AP_Var variables - // @name the name of the control system - AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_Board * board, - AP_ArmingMechanism * armingMechanism, - const uint8_t _chMode, - const uint16_t key, - const prog_char_t * name = NULL); - - /// - // The loop callback function. - // The callback function for the controller loop. - // This is inherited from loop. - // This function cannot be overriden. - // @dt The loop update interval. - void update(const float dt); - - /// - // This sets all radio outputs to neutral. - // This function cannot be overriden. - void setAllRadioChannelsToNeutral(); - - /// - // This sets all radio outputs using the radio input. - // This function cannot be overriden. - void setAllRadioChannelsManually(); - - /// - // Sets the motor pwm outputs. - // This function sets the motors given the control system outputs. - // This function must be defined. There is no default implementation. - virtual void setMotors() = 0; - - /// - // The manual control loop function. - // This uses radio to control the aircraft. - // This function must be defined. There is no default implementation. - // @dt The loop update interval. - virtual void manualLoop(const float dt) = 0; - - /// - // The automatic control update function. - // This loop is responsible for taking the - // vehicle to a waypoint. - // This function must be defined. There is no default implementation. - // @dt The loop update interval. - virtual void autoLoop(const float dt) = 0; - - /// - // Handles failsafe events. - // This function is responsible for setting the mode of the vehicle during - // a failsafe event (low battery, loss of gcs comms, ...). - // This function must be defined. There is no default implementation. - virtual void handleFailsafe() = 0; - - /// - // The mode accessor. - // @return The current vehicle mode. - MAV_MODE getMode() { - return _mode; - } - /// - // The mode setter. - // @mode The mode to set the vehicle to. - void setMode(MAV_MODE mode) { - _mode = mode; - } - /// - // The state acessor. - // @return The current state of the vehicle. - MAV_STATE getState() const { - return _state; - } - /// - // state setter - // @sate The state to set the vehicle to. - void setState(MAV_STATE state) { - _state = state; - } - -protected: - AP_Navigator * _nav; /// navigator - AP_Guide * _guide; /// guide - AP_Board * _board; /// hardware abstraction layer - AP_ArmingMechanism * _armingMechanism; /// controls arming/ disarming - uint8_t _chMode; /// the channel the mode switch is on - AP_Var_group _group; /// holds controller parameters - MAV_MODE _mode; /// vehicle mode (auto, guided, manual, failsafe, ...) - MAV_STATE _state; /// vehicle state (active, standby, boot, calibrating ...) -}; - -} // apo - -#endif // AP_Controller_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ControllerBlock.cpp b/libraries/APO/AP_ControllerBlock.cpp deleted file mode 100644 index 785df2de7e..0000000000 --- a/libraries/APO/AP_ControllerBlock.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* - * AP_ControllerBlock.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "AP_ControllerBlock.h" -#include - -namespace apo { - -AP_ControllerBlock::AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, - uint8_t groupLength) : - _group(group), _groupStart(groupStart), - _groupEnd(groupStart + groupLength) { -} - -BlockLowPass::BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, - const prog_char_t * fCutLabel) : - AP_ControllerBlock(group, groupStart, 1), - _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), - _y(0) { -} -float BlockLowPass::update(const float & input, const float & dt) { - float RC = 1 / (2 * M_PI * _fCut); // low pass filter - _y = _y + (input - _y) * (dt / (dt + RC)); - return _y; -} - -BlockSaturation::BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, - const prog_char_t * yMaxLabel) : - AP_ControllerBlock(group, groupStart, 1), - _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { -} -float BlockSaturation::update(const float & input) { - - // pid sum - float y = input; - - // saturation - if (y > _yMax) - y = _yMax; - if (y < -_yMax) - y = -_yMax; - return y; -} - -BlockDerivative::BlockDerivative() : - _lastInput(0), firstRun(true) { -} -float BlockDerivative::update(const float & input, const float & dt) { - float derivative = (input - _lastInput) / dt; - _lastInput = input; - if (firstRun) { - firstRun = false; - return 0; - } else - return derivative; -} - -BlockIntegral::BlockIntegral() : - _i(0) { -} -float BlockIntegral::update(const float & input, const float & dt) { - _i += input * dt; - return _i; -} - -BlockP::BlockP(AP_Var_group * group, uint8_t groupStart, float kP, - const prog_char_t * kPLabel) : - AP_ControllerBlock(group, groupStart, 1), - _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { -} - -float BlockP::update(const float & input) { - return _kP * input; -} - -BlockI::BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, - const prog_char_t * kILabel, - const prog_char_t * iMaxLabel) : - AP_ControllerBlock(group, groupStart, 2), - _kI(group, groupStart, kI, kILabel ? : PSTR("i")), - _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), - _eI(0) { -} - -float BlockI::update(const float & input, const float & dt) { - // integral - _eI += input * dt; - _eI = _blockSaturation.update(_eI); - return _kI * _eI; -} - -BlockD::BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, - const prog_char_t * kDLabel, - const prog_char_t * dFCutLabel) : - AP_ControllerBlock(group, groupStart, 2), - _blockLowPass(group, groupStart, dFCut, - dFCutLabel ? : PSTR("dFCut")), - _kD(group, _blockLowPass.getGroupEnd(), kD, - kDLabel ? : PSTR("d")), _eP(0) { -} -float BlockD::update(const float & input, const float & dt) { - // derivative with low pass - float _eD = _blockLowPass.update((input - _eP) / dt, dt); - // proportional, note must come after derivative - // because derivatve uses _eP as previous value - _eP = input; - return _kD * _eD; -} - -BlockPID::BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut) : - AP_ControllerBlock(group, groupStart, 6), - _blockP(group, groupStart, kP), - _blockI(group, _blockP.getGroupEnd(), kI, iMax), - _blockD(group, _blockI.getGroupEnd(), kD, dFcut), - _blockSaturation(group, _blockD.getGroupEnd(), yMax) { -} - -float BlockPID::update(const float & input, const float & dt) { - return _blockSaturation.update( - _blockP.update(input) + _blockI.update(input, dt) - + _blockD.update(input, dt)); -} - -BlockPI::BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float iMax, float yMax) : - AP_ControllerBlock(group, groupStart, 4), - _blockP(group, groupStart, kP), - _blockI(group, _blockP.getGroupEnd(), kI, iMax), - _blockSaturation(group, _blockI.getGroupEnd(), yMax) { -} - -float BlockPI::update(const float & input, const float & dt) { - - float y = _blockP.update(input) + _blockI.update(input, dt); - return _blockSaturation.update(y); -} - -BlockPD::BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut) : - AP_ControllerBlock(group, groupStart, 4), - _blockP(group, groupStart, kP), - _blockD(group, _blockP.getGroupEnd(), kD, dFcut), - _blockSaturation(group, _blockD.getGroupEnd(), yMax) { -} - -float BlockPD::update(const float & input, const float & dt) { - - float y = _blockP.update(input) + _blockD.update(input, dt); - return _blockSaturation.update(y); -} - -BlockPIDDfb::BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, - const prog_char_t * dLabel) : - AP_ControllerBlock(group, groupStart, 5), - _blockP(group, groupStart, kP), - _blockI(group, _blockP.getGroupEnd(), kI, iMax), - _blockSaturation(group, _blockI.getGroupEnd(), yMax), - _kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d")) -{ -} -float BlockPIDDfb::update(const float & input, const float & derivative, - const float & dt) { - - float y = _blockP.update(input) + _blockI.update(input, dt) + _kD - * derivative; - return _blockSaturation.update(y); -} - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ControllerBlock.h b/libraries/APO/AP_ControllerBlock.h deleted file mode 100644 index 23383554cd..0000000000 --- a/libraries/APO/AP_ControllerBlock.h +++ /dev/null @@ -1,235 +0,0 @@ -/* - * AP_ControllerBlock.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_ControllerBlock_H -#define AP_ControllerBlock_H - -// inclusions -#include "../AP_Common/AP_Common.h" -#include "../AP_Common/AP_Var.h" - -// ArduPilotOne namespace -namespace apo { - -/// -// The abstract class defining a controller block. -class AP_ControllerBlock { -public: - /// - // Controller block constructor. - // This creates a controller block. - // @group The group containing the class parameters. - // @groupStart The start of the group. Used for chaining parameters. - // @groupEnd The end of the group. - AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, - uint8_t groupLength); - - /// - // Get the end of the AP_Var group. - // This is used for chaining multiple AP_Var groups into one. - uint8_t getGroupEnd() { - return _groupEnd; - } -protected: - AP_Var_group * _group; /// Contains all the parameters of the class. - uint8_t _groupStart; /// The start of the AP_Var group. Used for chaining parameters. - uint8_t _groupEnd; /// The end of the AP_Var group. -}; - -/// -// A low pass filter block. -// This takes a signal and smooths it out. It -// cuts all frequencies higher than the frequency provided. -class BlockLowPass: public AP_ControllerBlock { -public: - /// - // The constructor. - // @group The group containing the class parameters. - // @groupStart The start of the group. Used for chaining parameters. - // @fCut The cut-off frequency in Hz for smoothing. - BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, - const prog_char_t * fCutLabel = NULL); - - /// - // The update function. - // @input The input signal. - // @dt The timer interval. - // @return The output (smoothed) signal. - float update(const float & input, const float & dt); -protected: - AP_Float _fCut; /// The cut-off frequency in Hz. - float _y; /// The internal state of the low pass filter. -}; - -/// -// This block saturates a signal. -// If the signal is above max it is set to max. -// If it is below -max it is set to -max. -class BlockSaturation: public AP_ControllerBlock { -public: - /// - // Controller block constructor. - // This creates a controller block. - // @group The group containing the class parameters. - // @groupStart The start of the group. Used for chaining parameters. - // @yMax The maximum threshold. - BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, - const prog_char_t * yMaxLabel = NULL); - /// - // The update function. - // @input The input signal. - // @return The output (saturated) signal. - float update(const float & input); -protected: - AP_Float _yMax; /// output saturation -}; - -/// -// This block calculates a derivative. -class BlockDerivative { -public: - /// The constructor. - BlockDerivative(); - - /// - // The update function. - // @input The input signal. - // @return The derivative. - float update(const float & input, const float & dt); -protected: - float _lastInput; /// The last input to the block. - bool firstRun; /// Keeps track of first run inorder to decide if _lastInput should be used. -}; - -/// This block calculates an integral. -class BlockIntegral { -public: - /// The constructor. - BlockIntegral(); - /// - // The update function. - // @input The input signal. - // @dt The timer interval. - // @return The output (integrated) signal. - float update(const float & input, const float & dt); -protected: - float _i; /// integral -}; - -/// -// This is a proportional block with built-in gain. -class BlockP: public AP_ControllerBlock { -public: - BlockP(AP_Var_group * group, uint8_t groupStart, float kP, - const prog_char_t * kPLabel = NULL); - /// - // The update function. - // @input The input signal. - // @return The output signal (kP*input). - float update(const float & input); -protected: - AP_Float _kP; /// proportional gain -}; - -/// -// This is a integral block with built-in gain. -class BlockI: public AP_ControllerBlock { -public: - BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, - const prog_char_t * kILabel = NULL, - const prog_char_t * iMaxLabel = NULL); - float update(const float & input, const float & dt); -protected: - AP_Float _kI; /// integral gain - BlockSaturation _blockSaturation; /// integrator saturation - float _eI; /// internal integrator state -}; - -/// -// This is a derivative block with built-in gain. -class BlockD: public AP_ControllerBlock { -public: - BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, - const prog_char_t * kDLabel = NULL, - const prog_char_t * dFCutLabel = NULL); - float update(const float & input, const float & dt); -protected: - BlockLowPass _blockLowPass; /// The low-pass filter block - AP_Float _kD; /// The derivative gain - float _eP; /// The previous state -}; - -/// -// This is a proportional, integral, derivative block with built-in gains. -class BlockPID: public AP_ControllerBlock { -public: - BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut); - float update(const float & input, const float & dt); -protected: - BlockP _blockP; /// The proportional block. - BlockI _blockI; /// The integral block. - BlockD _blockD; /// The derivative block. - BlockSaturation _blockSaturation; /// The saturation block. -}; - -/// -// This is a proportional, integral block with built-in gains. -class BlockPI: public AP_ControllerBlock { -public: - BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float iMax, float yMax); - float update(const float & input, const float & dt); -protected: - BlockP _blockP; /// The proportional block. - BlockI _blockI; /// The derivative block. - BlockSaturation _blockSaturation; /// The saturation block. -}; - -/// -// This is a proportional, derivative block with built-in gains. -class BlockPD: public AP_ControllerBlock { -public: - BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut); - float update(const float & input, const float & dt); -protected: - BlockP _blockP; /// The proportional block. - BlockD _blockD; /// The derivative block. - BlockSaturation _blockSaturation; /// The saturation block. -}; - -/// PID with derivative feedback (ignores reference derivative) -class BlockPIDDfb: public AP_ControllerBlock { -public: - BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, - const prog_char_t * dLabel = NULL); - float update(const float & input, const float & derivative, - const float & dt); -protected: - BlockP _blockP; /// The proportional block. - BlockI _blockI; /// The integral block. - BlockSaturation _blockSaturation; /// The saturation block. - AP_Float _kD; /// derivative gain -}; - -} // apo - -#endif // AP_ControllerBlock_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp deleted file mode 100644 index 82a46d1a24..0000000000 --- a/libraries/APO/AP_Guide.cpp +++ /dev/null @@ -1,236 +0,0 @@ -/* - * AP_Guide.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "AP_Guide.h" -#include "../FastSerial/FastSerial.h" -#include "AP_Navigator.h" -#include "constants.h" -#include "AP_Board.h" -#include "AP_CommLink.h" - -namespace apo { - -AP_Guide::AP_Guide(AP_Navigator * nav, AP_Board * board) : - _nav(nav), _board(board), _command(AP_MavlinkCommand::home), - _previousCommand(AP_MavlinkCommand::home), - _headingCommand(0), _airSpeedCommand(0), - _groundSpeedCommand(0), _altitudeCommand(0), - _mode(MAV_NAV_LOST), - _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0), - _nextCommandTimer(0) { -} - -void AP_Guide::setCurrentIndex(uint8_t val) { - _cmdIndex.set_and_save(val); - _command = AP_MavlinkCommand(getCurrentIndex()); - _previousCommand = AP_MavlinkCommand(getPreviousIndex()); - _board->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); - updateCommand(); -} - -float AP_Guide::getHeadingError() { - return wrapAngle(getHeadingCommand() - - _nav->getYaw()); -} - -float AP_Guide::getDistanceToNextWaypoint() { - return _command.distanceTo(_nav->getLat_degInt(), - _nav->getLon_degInt()); -} - -float AP_Guide::getGroundSpeedError() { - return _groundSpeedCommand - _nav->getGroundSpeed(); -} - -MavlinkGuide::MavlinkGuide(AP_Navigator * nav, - AP_Board * board, float velCmd, float xt, float xtLim) : - AP_Guide(nav, board), - _group(k_guide, PSTR("guide_")), - _velocityCommand(&_group, 1, velCmd, PSTR("velCmd")), - _crossTrackGain(&_group, 2, xt, PSTR("xt")), - _crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) { -} - -float AP_Guide::getYawError(){ - return wrapAngle(_yawCommand - _nav->getYaw()); -} - -void MavlinkGuide::update() { - // process mavlink commands - handleCommand(); -} - -float MavlinkGuide::getPNError() { - return -_command.getPN(_nav->getLat_degInt(), _nav->getLon_degInt()); -} -float MavlinkGuide::getPEError() { - return -_command.getPE(_nav->getLat_degInt(), _nav->getLon_degInt()); -} -float MavlinkGuide::getPDError() { - return -_command.getPD(_nav->getAlt_intM()); -} - -void MavlinkGuide::nextCommand() { - // within 1 seconds, check if more than 5 calls to next command occur - // if they do, go to home waypoint - if (millis() - _nextCommandTimer < 1000) { - if (_nextCommandCalls > 5) { - Serial.println("commands loading too fast, returning home"); - setCurrentIndex(0); - setNumberOfCommands(1); - _nextCommandCalls = 0; - _nextCommandTimer = millis(); - return; - } - _nextCommandCalls++; - } else { - _nextCommandTimer = millis(); - _nextCommandCalls = 0; - } - - // set the current command - setCurrentIndex(getNextIndex()); -} - -void MavlinkGuide::updateCommand() { - // update guidance mode - if (_command.getCommand() == MAV_CMD_NAV_WAYPOINT) { - _mode = MAV_NAV_WAYPOINT; - } else if (_command.getCommand() == MAV_CMD_NAV_LAND) { - _mode = MAV_NAV_LANDING; - } else if (_command.getCommand() == MAV_CMD_NAV_LOITER_TIME) { - _mode = MAV_NAV_LOITER; - } else if (_command.getCommand() == MAV_CMD_NAV_LOITER_UNLIM) { - _mode = MAV_NAV_LOITER; - } else if (_command.getCommand() == MAV_CMD_NAV_LOITER_TURNS) { - _mode = MAV_NAV_LOITER; - } else if (_command.getCommand() == MAV_CMD_NAV_RETURN_TO_LAUNCH) { - _mode = MAV_NAV_RETURNING; - } else if (_command.getCommand() == MAV_CMD_NAV_TAKEOFF) { - _mode = MAV_NAV_LIFTOFF; - } else { - _board->debug->printf_P(PSTR("unhandled command")); - _board->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled command")); - nextCommand(); - return; - } - - // TODO handle more commands - //MAV_CMD_CONDITION_CHANGE_ALT - //MAV_CMD_CONDITION_DELAY - //MAV_CMD_CONDITION_DISTANCE - //MAV_CMD_CONDITION_LAST - //MAV_CMD_CONDITION_YAW - - //MAV_CMD_DO_CHANGE_SPEED - //MAV_CMD_DO_CONTROL_VIDEO - //MAV_CMD_DO_JUMP - //MAV_CMD_DO_LAST - //MAV_CMD_DO_LAST - //MAV_CMD_DO_REPEAT_RELAY - //MAV_CMD_DO_REPEAT_SERVO - //MAV_CMD_DO_SET_HOME - //MAV_CMD_DO_SET_MODE - //MAV_CMD_DO_SET_PARAMETER - //MAV_CMD_DO_SET_RELAY - //MAV_CMD_DO_SET_SERVO - - //MAV_CMD_PREFLIGHT_CALIBRATION - //MAV_CMD_PREFLIGHT_STORAGE -} - -void MavlinkGuide::handleCommand() { - - // for these modes use crosstrack navigation - if ( - _mode == MAV_NAV_WAYPOINT || - _mode == MAV_NAV_LANDING || - _mode == MAV_NAV_LIFTOFF || - _mode == MAV_NAV_VECTOR) { - - // if we don't have enough waypoint for cross track calcs - // switch to loiter mode - if (_numberOfCommands == 1) { - _mode = MAV_NAV_LOITER; - return; - } - - // check if we are at waypoint or if command - // radius is zero within tolerance - if ( (getDistanceToNextWaypoint() < _command.getRadius()) | (getDistanceToNextWaypoint() < 1e-5) ) { - _board->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); - _board->debug->printf_P(PSTR("waypoint reached (distance)")); - nextCommand(); - return; - } - - // check for along track next waypoint requirement - float alongTrack = _command.alongTrack(_previousCommand, - _nav->getLat_degInt(), - _nav->getLon_degInt()); - float segmentLength = _previousCommand.distanceTo(_command); - if (alongTrack > segmentLength) { - _board->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (along track)")); - _board->debug->printf_P(PSTR("waypoint reached (along track) segmentLength: %f\t alongTrack: %f\n"),segmentLength,alongTrack); - nextCommand(); - return; - } - - // calculate altitude and heading commands - _altitudeCommand = _command.getAlt(); - float dXt = _command.crossTrack(_previousCommand, - _nav->getLat_degInt(), - _nav->getLon_degInt()); - float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m - if (temp > _crossTrackLim * deg2Rad) - temp = _crossTrackLim * deg2Rad; - if (temp < -_crossTrackLim * deg2Rad) - temp = -_crossTrackLim * deg2Rad; - float bearing = _previousCommand.bearingTo(_command); - _headingCommand = bearing - temp; - _yawCommand = _command.getYawCommand(); - _board->debug->printf_P( - PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\tyaw command: %f\n"), - bearing * rad2Deg, dXt, _headingCommand * rad2Deg, getDistanceToNextWaypoint(), alongTrack, _yawCommand*rad2Deg); - - // for these modes just head to current command - } else if ( - _mode == MAV_NAV_LOITER || - _mode == MAV_NAV_RETURNING) { - _altitudeCommand = AP_MavlinkCommand::home.getAlt(); - _headingCommand = AP_MavlinkCommand::home.bearingTo( - _nav->getLat_degInt(), _nav->getLon_degInt()) - + 180 * deg2Rad; - if (_headingCommand > 360 * deg2Rad) - _headingCommand -= 360 * deg2Rad; - - // do nothing for these modes - } else if ( - _mode == MAV_NAV_GROUNDED || - _mode == MAV_NAV_HOLD || - _mode == MAV_NAV_LOST) { - - } - - // if in unhandled mode, then return - else { - _board->debug->printf_P(PSTR("unhandled guide mode")); - _board->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled guide mode")); - _mode = MAV_NAV_RETURNING; - } - - _groundSpeedCommand = _velocityCommand; - - // debug - //_board->debug->printf_P( - //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), - //getNumberOfCommands(), getCurrentIndex(), getPreviousIndex()); -} - -} // namespace apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h deleted file mode 100644 index 86df9d6c92..0000000000 --- a/libraries/APO/AP_Guide.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * AP_Guide.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_Guide_H -#define AP_Guide_H - -#include -#include "../GCS_MAVLink/GCS_MAVLink.h" -#include "AP_MavlinkCommand.h" -#include "../AP_RangeFinder/AP_RangeFinder.h" - -namespace apo { - -class AP_Navigator; -class AP_Board; - -/// Guide class -class AP_Guide { -public: - - /** - * This is the constructor, which requires a link to the navigator. - * @param navigator This is the navigator pointer. - */ - AP_Guide(AP_Navigator * nav, AP_Board * board); - - virtual void update() = 0; - - virtual void nextCommand() = 0; - - virtual void updateCommand() {}; - - MAV_NAV getMode() const { - return _mode; - } - - void setMode(MAV_NAV mode) { - _mode = mode; - } - - uint8_t getCurrentIndex() { - return _cmdIndex; - } - - void setCurrentIndex(uint8_t val); - - uint8_t getNumberOfCommands() { - return _numberOfCommands; - } - - void setNumberOfCommands(uint8_t val) { - _numberOfCommands.set_and_save(val); - } - - uint8_t getPreviousIndex() { - // find previous waypoint, TODO, handle non-nav commands - int16_t prevIndex = int16_t(getCurrentIndex()) - 1; - if (prevIndex < 0) - prevIndex = getNumberOfCommands() - 1; - return (uint8_t) prevIndex; - } - - uint8_t getNextIndex() { - // find previous waypoint, TODO, handle non-nav commands - int16_t nextIndex = int16_t(getCurrentIndex()) + 1; - if (nextIndex > (getNumberOfCommands() - 1)) - nextIndex = 0; - return nextIndex; - } - - float getHeadingError(); - - /// the commanded course over ground for the vehicle - float getHeadingCommand() { - return _headingCommand; - } - - /// wrap an angle between -180 and 180 - float wrapAngle(float y) { - if (y > 180 * deg2Rad) - y -= 360 * deg2Rad; - if (y < -180 * deg2Rad) - y += 360 * deg2Rad; - return y; - } - - /// the yaw attitude error of the vehicle - float getYawError(); - - float getAirSpeedCommand() { - return _airSpeedCommand; - } - float getGroundSpeedCommand() { - return _groundSpeedCommand; - } - float getGroundSpeedError(); - - float getAltitudeCommand() { - return _altitudeCommand; - } - float getDistanceToNextWaypoint(); - - virtual float getPNError() = 0; - virtual float getPEError() = 0; - virtual float getPDError() = 0; - - MAV_NAV getMode() { - return _mode; - } - uint8_t getCommandIndex() { - return _cmdIndex; - } - -protected: - AP_Navigator * _nav; - AP_Board * _board; - AP_MavlinkCommand _command, _previousCommand; - float _headingCommand; - float _yawCommand; - float _airSpeedCommand; - float _groundSpeedCommand; - float _altitudeCommand; - MAV_NAV _mode; - AP_Uint8 _numberOfCommands; - AP_Uint8 _cmdIndex; - uint16_t _nextCommandCalls; - uint16_t _nextCommandTimer; -}; - -class MavlinkGuide: public AP_Guide { -public: - MavlinkGuide(AP_Navigator * nav, - AP_Board * board, float velCmd, float xt, float xtLim); - virtual void update(); - void nextCommand(); - void handleCommand(); - void updateCommand(); - virtual float getPNError(); - virtual float getPEError(); - virtual float getPDError(); - -private: - AP_Var_group _group; - AP_Float _velocityCommand; - AP_Float _crossTrackGain; - AP_Float _crossTrackLim; -}; - -} // namespace apo - -#endif // AP_Guide_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Loop.cpp b/libraries/APO/AP_Loop.cpp deleted file mode 100644 index 031c59c982..0000000000 --- a/libraries/APO/AP_Loop.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * AP_Loop.pde - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#include "AP_Loop.h" - -Loop::Loop(float _frequency, void (*fptr)(void *), void * data) : - _fptr(fptr), - _data(data), - _period(1.0e6/_frequency), - _subLoops(), - _timeStamp(micros()), - _load(0), - _dt(0) -{ -} - -void Loop::update() -{ - // quick exit if not ready - if (micros() - _timeStamp < _period) return; - - // update time stamp - uint32_t timeStamp0 = _timeStamp; - _timeStamp = micros(); - _dt = (_timeStamp - timeStamp0)/1.0e6; - - // update sub loops - for (uint8_t i=0; i<_subLoops.getSize(); i++) _subLoops[i]->update(); - - // callback function - if (_fptr) _fptr(_data); - - // calculated load with a low pass filter - _load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0)); -} - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Loop.h b/libraries/APO/AP_Loop.h deleted file mode 100644 index c2baaa32c7..0000000000 --- a/libraries/APO/AP_Loop.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * AP_Loop.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_Loop_H -#define AP_Loop_H - -#include "AP_Vector.h" - -class Loop -{ -public: - Loop() : _fptr(), _data(), _period(), _subLoops(), _timeStamp(), _load(), _dt() { - }; - Loop(float frequency, void (*fptr)(void *) = NULL, void * data = NULL); - void update(); - Vector & subLoops() { - return _subLoops; - } - float frequency() { - return 1.0e6/_period; - } - void frequency(float _frequency) { - _period = 1e6/_frequency; - } - uint32_t timeStamp() { - return _timeStamp; - } - float dt() { - return _dt; - } - uint8_t load() { - return _load; - } -protected: - void (*_fptr)(void *); - void * _data; - uint32_t _period; - Vector _subLoops; - uint32_t _timeStamp; - uint8_t _load; - float _dt; -}; - -#endif - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp deleted file mode 100644 index 5b976edb30..0000000000 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/* - * AP_MavlinkCommand.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "../FastSerial/FastSerial.h" -#include "AP_MavlinkCommand.h" - -namespace apo { - -AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) : - _data(v._data), _seq(v._seq) { - //if (FastSerial::getInitialized(0)) Serial.println("copy ctor"); -} - -AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) : - _data(k_commands + index), _seq(index) { - - if (FastSerial::getInitialized(0)) { - Serial.println("index ctor"); - Serial.println("++"); - Serial.print("index: "); - Serial.println(index); - Serial.print("key: "); - Serial.println(k_commands + index); - Serial.println("++"); - } - - // default values for structure - _data.get().command = MAV_CMD_NAV_WAYPOINT; - _data.get().autocontinue = true; - _data.get().frame = MAV_FRAME_GLOBAL; - _data.get().param1 = 0; - _data.get().param2 = 10; // radius of 10 meters - _data.get().param3 = 0; - _data.get().param4 = 0; - _data.get().x = 0; - _data.get().y = 0; - _data.get().z = 1000; - - // This is a failsafe measure to stop trying to load a command if it can't load - if (doLoad && !load()) { - Serial.println("load failed, reverting to home waypoint"); - _data = AP_MavlinkCommand::home._data; - _seq = AP_MavlinkCommand::home._seq; - } -} - -AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) : - _data(k_commands + cmd.seq), _seq(cmd.seq) { - setCommand(MAV_CMD(cmd.command)); - setAutocontinue(cmd.autocontinue); - setFrame(MAV_FRAME(cmd.frame)); - setParam1(cmd.param1); - setParam2(cmd.param2); - setParam3(cmd.param3); - setParam4(cmd.param4); - setX(cmd.x); - setY(cmd.y); - setZ(cmd.z); - save(); - - // reload home if sent, home must be a global waypoint - if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) home.load(); - - Serial.println("============================================================"); - Serial.println("storing new command from mavlink_waypoint_t"); - Serial.print("key: "); - Serial.println(_data.key(),DEC); - Serial.print("number: "); - Serial.println(cmd.seq,DEC); - Serial.print("command: "); - Serial.println(getCommand()); - Serial.print("autocontinue: "); - Serial.println(getAutocontinue(),DEC); - Serial.print("frame: "); - Serial.println(getFrame(),DEC); - Serial.print("1000*param1: "); - Serial.println(int(1000*getParam1()),DEC); - Serial.print("1000*param2: "); - Serial.println(int(1000*getParam2()),DEC); - Serial.print("1000*param3: "); - Serial.println(int(1000*getParam3()),DEC); - Serial.print("1000*param4: "); - Serial.println(int(1000*getParam4()),DEC); - Serial.print("1000*x0: "); - Serial.println(int(1000*cmd.x),DEC); - Serial.print("1000*y0: "); - Serial.println(int(1000*cmd.y),DEC); - Serial.print("1000*z0: "); - Serial.println(int(1000*cmd.z),DEC); - Serial.print("1000*x: "); - Serial.println(int(1000*getX()),DEC); - Serial.print("1000*y: "); - Serial.println(int(1000*getY()),DEC); - Serial.print("1000*z: "); - Serial.println(int(1000*getZ()),DEC); - - load(); - - Serial.print("1000*x1: "); - Serial.println(int(1000*getX()),DEC); - Serial.print("1000*y1: "); - Serial.println(int(1000*getY()),DEC); - Serial.print("1000*z1: "); - Serial.println(int(1000*getZ()),DEC); - Serial.println("============================================================"); - Serial.flush(); -} - -mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) const { - mavlink_waypoint_t mavCmd; - mavCmd.seq = getSeq(); - mavCmd.command = getCommand(); - mavCmd.frame = getFrame(); - mavCmd.param1 = getParam1(); - mavCmd.param2 = getParam2(); - mavCmd.param3 = getParam3(); - mavCmd.param4 = getParam4(); - mavCmd.x = getX(); - mavCmd.y = getY(); - mavCmd.z = getZ(); - mavCmd.autocontinue = getAutocontinue(); - mavCmd.current = (getSeq() == current); - mavCmd.target_component = mavlink_system.compid; - mavCmd.target_system = mavlink_system.sysid; - return mavCmd; -} - -float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const { - float deltaLon = next.getLon() - getLon(); - /* - Serial.print("Lon: "); Serial.println(getLon()); - Serial.print("nextLon: "); Serial.println(next.getLon()); - Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); - */ - float bearing = atan2( - sin(deltaLon) * cos(next.getLat()), - cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos( - next.getLat()) * cos(deltaLon)); - return bearing; -} - -float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const { - // have to be careful to maintain the precision of the gps coordinate - float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad; - float nextLat = latDegInt * degInt2Rad; - float bearing = atan2( - sin(deltaLon) * cos(nextLat), - cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat) - * cos(deltaLon)); - if (bearing < 0) - bearing += 2 * M_PI; - return bearing; -} - -float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const { - float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2); - float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2); - float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( - next.getLat()) * sinDeltaLon2 * sinDeltaLon2; - float c = 2 * atan2(sqrt(a), sqrt(1 - a)); - return rEarth * c; -} - -float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const { - float sinDeltaLat2 = sin( - (lat_degInt - getLat_degInt()) * degInt2Rad / 2); - float sinDeltaLon2 = sin( - (lon_degInt - getLon_degInt()) * degInt2Rad / 2); - float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( - lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2; - float c = 2 * atan2(sqrt(a), sqrt(1 - a)); - /* - Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); - Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); - Serial.print("lat_degInt: "); Serial.println(lat_degInt); - Serial.print("lon_degInt: "); Serial.println(lon_degInt); - Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); - Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); - */ - return rEarth * c; -} - -//calculates cross track of a current location -float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous, - int32_t lat_degInt, int32_t lon_degInt) const { - float d = previous.distanceTo(lat_degInt, lon_degInt); - float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); - float bNext = previous.bearingTo(*this); - return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; -} - -// calculates along track distance of a current location -float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, - int32_t lat_degInt, int32_t lon_degInt) const { - float t1N = previous.getPN(lat_degInt, lon_degInt); - float t1E = previous.getPE(lat_degInt, lon_degInt); - float t2N = previous.getPN(getLat_degInt(), getLon_degInt()); - float t2E = previous.getPE(getLat_degInt(), getLon_degInt()); - float segmentLength = previous.distanceTo(*this); - if (segmentLength == 0) return 0; - return (t1N*t2N + t1E*t2E)/segmentLength; -} - - -AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false); - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h deleted file mode 100644 index c6d4ee2e46..0000000000 --- a/libraries/APO/AP_MavlinkCommand.h +++ /dev/null @@ -1,380 +0,0 @@ -/* - * AP_MavlinkCommand.h - * - * Created on: Apr 4, 2011 - * Author: jgoppert - */ - -#ifndef AP_MAVLINKCOMMAND_H_ -#define AP_MAVLINKCOMMAND_H_ - -#include "../GCS_MAVLink/GCS_MAVLink.h" -#include "../AP_Common/AP_Common.h" -#include "AP_Var_keys.h" -#include "constants.h" - -namespace apo { - -class AP_MavlinkCommand { -private: - struct CommandStorage { - MAV_CMD command; - bool autocontinue; - MAV_FRAME frame; - float param1; - float param2; - float param3; - float param4; - float x; - float y; - float z; - }; - AP_VarS _data; - uint16_t _seq; -public: - static AP_MavlinkCommand home; - - /** - * Copy Constructor - */ - AP_MavlinkCommand(const AP_MavlinkCommand & v); - - /** - * Basic Constructor - * @param index Start at zero. - */ - AP_MavlinkCommand(uint16_t index, bool doLoad = true); - - /** - * Constructor for copying/ saving from a mavlink waypoint. - * @param cmd The mavlink_waopint_t structure for the command. - */ - AP_MavlinkCommand(const mavlink_waypoint_t & cmd); - - bool save() { - return _data.save(); - } - bool load() { - return _data.load(); - } - uint8_t getSeq() const { - return _seq; - } - bool getAutocontinue() const { - return _data.get().autocontinue; - } - void setAutocontinue( bool val) { - _data.get().autocontinue = val; - } - void setSeq(uint8_t val) { - _seq = val; - } - MAV_CMD getCommand() const { - return _data.get().command; - } - void setCommand(MAV_CMD val) { - _data.get().command = val; - } - MAV_FRAME getFrame() const { - return _data.get().frame; - } - void setFrame(MAV_FRAME val) { - _data.get().frame = val; - } - float getParam1() const { - return _data.get().param1; - } - void setParam1(float val) { - _data.get().param1 = val; - } - float getParam2() const { - return _data.get().param2; - } - void setParam2(float val) { - _data.get().param2 = val; - } - float getParam3() const { - return _data.get().param3; - } - void setParam3(float val) { - _data.get().param3 = val; - } - float getParam4() const { - return _data.get().param4; - } - void setParam4(float val) { - _data.get().param4 = val; - } - float getX() const { - return _data.get().x; - } - void setX(float val) { - _data.get().x = val; - } - float getY() const { - return _data.get().y; - } - void setY(float val) { - _data.get().y = val; - } - float getZ() const { - return _data.get().z; - } - void setZ(float val) { - _data.get().z = val; - } - - float getYawCommand() const { - return deg2Rad*getParam4(); - } - - float getLatDeg() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return getX(); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - void setLatDeg(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setX(val); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - break; - } - } - float getLonDeg() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return getY(); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - void setLonDeg(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setY(val); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - break; - } - } - void setLon(float val) { - setLonDeg(val * rad2Deg); - } - void setLon_degInt(int32_t val) { - setLonDeg(val / 1.0e7); - } - void setLat_degInt(int32_t val) { - setLatDeg(val / 1.0e7); - } - int32_t getLon_degInt() const { - return getLonDeg() * 1e7; - } - int32_t getLat_degInt() const { - return getLatDeg() * 1e7; - } - float getLon() const { - return getLonDeg() * deg2Rad; - } - float getLat() const { - return getLatDeg() * deg2Rad; - } - void setLat(float val) { - setLatDeg(val * rad2Deg); - } - float getAlt() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - return getZ(); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - return -getZ() + home.getAlt(); - break; - case MAV_FRAME_LOCAL_ENU: - return getZ() + home.getAlt(); - break; - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - /** - * set the altitude in meters - */ - void setAlt(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setZ(val); - break; - case MAV_FRAME_LOCAL: - setZ(home.getLonDeg() - val); - break; - case MAV_FRAME_LOCAL_ENU: - setZ(val - home.getLonDeg()); - break; - case MAV_FRAME_MISSION: - default: - break; - } - } - /** - * Get the relative altitude to home - * @return relative altitude in meters - */ - float getRelAlt() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - return getZ() - home.getAlt(); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - return -getZ(); - break; - case MAV_FRAME_LOCAL_ENU: - return getZ(); - break; - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - /** - * set the relative altitude in meters from home (up) - */ - void setRelAlt(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - setZ(val + home.getAlt()); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - setZ(-val); - break; - case MAV_FRAME_LOCAL_ENU: - setZ(val); - break; - case MAV_FRAME_MISSION: - break; - } - } - - float getRadius() const { - return getParam2(); - } - - void setRadius(float val) { - setParam2(val); - } - - /** - * conversion for outbound packets to ground station - * @return output the mavlink_waypoint_t packet - */ - mavlink_waypoint_t convert(uint8_t current) const; - - /** - * Calculate the bearing from this command to the next command - * @param next The command to calculate the bearing to. - * @return the bearing - */ - float bearingTo(const AP_MavlinkCommand & next) const; - - /** - * Bearing form this command to a gps coordinate in integer units - * @param latDegInt latitude in degrees E-7 - * @param lonDegInt longitude in degrees E-7 - * @return - */ - float bearingTo(int32_t latDegInt, int32_t lonDegInt) const; - - /** - * Distance to another command - * @param next The command to measure to. - * @return The distance in meters. - */ - float distanceTo(const AP_MavlinkCommand & next) const; - - /** - * Distance to a gps coordinate in integer units - * @param latDegInt latitude in degrees E-7 - * @param lonDegInt longitude in degrees E-7 - * @return The distance in meters. - */ - float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const; - - float getPN(int32_t lat_degInt, int32_t lon_degInt) const { - // local tangent approximation at this waypoint - float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad; - return deltaLat * rEarth; - } - - float getPE(int32_t lat_degInt, int32_t lon_degInt) const { - // local tangent approximation at this waypoint - float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad; - return cos(getLat()) * deltaLon * rEarth; - } - - float getPD(int32_t alt_intM) const { - return -(alt_intM / scale_m - getAlt()); - } - - float getLat(float pN) const { - - return pN / rEarth + getLat(); - } - - float getLon(float pE) const { - - return pE / rEarth / cos(getLat()) + getLon(); - } - - /** - * Gets altitude in meters - * @param pD alt in meters - * @return - */ - float getAlt(float pD) const { - - return getAlt() - pD; - } - - //calculates cross track of a current location - float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; - - // calculates along track distance of a current location - float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; -}; - -} // namespace apo - - -#endif /* AP_MAVLINKCOMMAND_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp deleted file mode 100644 index 875b724eb7..0000000000 --- a/libraries/APO/AP_Navigator.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * AP_Navigator.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "AP_Navigator.h" -#include "AP_MavlinkCommand.h" - -namespace apo { - -AP_Navigator::AP_Navigator(AP_Board * board) : - _board(board), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), - _pitchRate(0), _yaw(0), _yawRate(0), - _windSpeed(0), _windDirection(0), - _vN(0), _vE(0), _vD(0), _lat_degInt(0), - _lon_degInt(0), _alt_intM(0) { -} -float AP_Navigator::getPD() const { - return AP_MavlinkCommand::home.getPD(getAlt_intM()); -} - -float AP_Navigator::getPE() const { - return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); -} - -float AP_Navigator::getPN() const { - return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); -} - -void AP_Navigator::setPD(float _pD) { - setAlt(AP_MavlinkCommand::home.getAlt(_pD)); -} - -void AP_Navigator::setPE(float _pE) { - setLat(AP_MavlinkCommand::home.getLat(_pE)); -} - -void AP_Navigator::setPN(float _pN) { - setLon(AP_MavlinkCommand::home.getLon(_pN)); -} - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h deleted file mode 100644 index 64761975b1..0000000000 --- a/libraries/APO/AP_Navigator.h +++ /dev/null @@ -1,297 +0,0 @@ -/* - * AP_Navigator.h - * Copyright (C) James Goppert 2010 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef AP_Navigator_H -#define AP_Navigator_H - -#include "constants.h" -#include - -namespace apo { - -class AP_Board; - -/// Navigator class -class AP_Navigator { -public: - AP_Navigator(AP_Board * board); - - // note, override these with derived navigator functionality - virtual void calibrate() {}; - virtual void updateFast(float dt) {}; - virtual void updateSlow(float dt) {}; - - - // accessors - float getPD() const; - float getPE() const; - float getPN() const; - void setPD(float _pD); - void setPE(float _pE); - void setPN(float _pN); - - float getAirSpeed() const { - // neglects vertical wind - float vWN = getVN() + getWindSpeed()*cos(getWindDirection()); - float vWE = getVE() + getWindSpeed()*sin(getWindDirection()); - return sqrt(vWN*vWN+vWE+vWE+getVD()*getVD()); - } - - float getGroundSpeed() const { - return sqrt(getVN()*getVN()+getVE()*getVE()); - } - - float getWindSpeed() const { - return _windSpeed; - } - - int32_t getAlt_intM() const { - return _alt_intM; - } - - float getAlt() const { - return _alt_intM / scale_m; - } - - void setAlt(float _alt) { - this->_alt_intM = _alt * scale_m; - } - - float getLat() const { - //Serial.print("getLatfirst"); - //Serial.println(_lat_degInt * degInt2Rad); - return _lat_degInt * degInt2Rad; - } - - void setLat(float _lat) { - //Serial.print("setLatfirst"); - //Serial.println(_lat * rad2DegInt); - setLat_degInt(_lat*rad2DegInt); - } - - float getLon() const { - return _lon_degInt * degInt2Rad; - - } - - void setLon(float _lon) { - this->_lon_degInt = _lon * rad2DegInt; - } - - float getVN() const { - return _vN; - } - - float getVE() const { - return _vE; - } - - float getVD() const { - return _vD; - } - - int32_t getLat_degInt() const { - //Serial.print("getLat_degInt"); - //Serial.println(_lat_degInt); - return _lat_degInt; - - } - - int32_t getLon_degInt() const { - return _lon_degInt; - } - - float getPitch() const { - return _pitch; - } - - float getPitchRate() const { - return _pitchRate; - } - - float getRoll() const { - return _roll; - } - - float getRollRate() const { - return _rollRate; - } - - float getYaw() const { - return _yaw; - } - - float getYawRate() const { - return _yawRate; - } - - float getWindDirection() const { - return _windDirection; - } - - float getCourseOverGround() const { - return atan2(getVE(),getVN()); - } - - float getRelativeCourseOverGround() const { - float y = getCourseOverGround() - getYaw(); - if (y > 180 * deg2Rad) - y -= 360 * deg2Rad; - if (y < -180 * deg2Rad) - y += 360 * deg2Rad; - return y; - } - - - float getSpeedOverGround() const { - return sqrt(getVN()*getVN()+getVE()*getVE()); - } - - float getXAccel() const { - return _xAccel; - } - - float getYAccel() const { - return _yAccel; - } - - float getZAccel() const { - return _zAccel; - } - - void setAirSpeed(float airSpeed) { - // assumes wind constant and rescale navigation speed - float vScale = (1 + airSpeed/getAirSpeed()); - float vNorm = sqrt(getVN()*getVN()+getVE()*getVE()+getVD()*getVD()); - _vN *= vScale/vNorm; - _vE *= vScale/vNorm; - _vD *= vScale/vNorm; - } - - void setAlt_intM(int32_t alt_intM) { - _alt_intM = alt_intM; - } - - void setVN(float vN) { - _vN = vN; - } - - void setVE(float vE) { - _vE = vE; - } - - void setVD(float vD) { - _vD = vD; - } - - void setXAccel(float xAccel) { - _xAccel = xAccel; - } - - void setYAccel(float yAccel) { - _yAccel = yAccel; - } - - void setZAccel(float zAccel) { - _zAccel = zAccel; - } - - void setGroundSpeed(float groundSpeed) { - float cog = getCourseOverGround(); - _vN = cos(cog)*groundSpeed; - _vE = sin(cog)*groundSpeed; - } - - void setLat_degInt(int32_t lat_degInt) { - _lat_degInt = lat_degInt; - //Serial.print("setLat_degInt"); - //Serial.println(_lat_degInt); - } - - void setLon_degInt(int32_t lon_degInt) { - _lon_degInt = lon_degInt; - } - - void setPitch(float pitch) { - _pitch = pitch; - } - - void setPitchRate(float pitchRate) { - _pitchRate = pitchRate; - } - - void setRoll(float roll) { - _roll = roll; - } - - void setRollRate(float rollRate) { - _rollRate = rollRate; - } - - void setYaw(float yaw) { - _yaw = yaw; - } - - void setYawRate(float yawRate) { - _yawRate = yawRate; - } - - void setTimeStamp(int32_t timeStamp) { - _timeStamp = timeStamp; - } - - int32_t getTimeStamp() const { - return _timeStamp; - } - - void setWindDirection(float windDirection) { - _windDirection = windDirection; - } - - void setWindSpeed(float windSpeed) { - _windSpeed = windSpeed; - } - -protected: - AP_Board * _board; -private: - int32_t _timeStamp; /// time stamp for navigation data, micros clock - float _roll; /// roll angle, radians - float _rollRate; /// roll rate, radians/s - float _pitch; /// pitch angle, radians - float _pitchRate; /// pitch rate, radians/s - float _yaw; /// yaw angle, radians - float _yawRate; /// yaw rate, radians/s - // vertical - float _windSpeed; /// wind speed, m/s - float _windDirection; /// wind directioin, radians - float _vN; /// - float _vE; - float _vD; // m/s - float _xAccel; - float _yAccel; - float _zAccel; - int32_t _lat_degInt; // deg / 1e7 - int32_t _lon_degInt; // deg / 1e7 - int32_t _alt_intM; // meters / 1e3 -}; - -} // namespace apo - -#endif // AP_Navigator_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp deleted file mode 100644 index 96603b594c..0000000000 --- a/libraries/APO/AP_RcChannel.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - AP_RcChannel.cpp - Radio library for Arduino - Code by Jason Short, James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and / or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - */ - -#include -#include -#include "AP_RcChannel.h" -#include "../AP_Common/AP_Common.h" -#include - -namespace apo { - -AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, - APM_RC_Class * rc, const uint8_t & ch, const uint16_t & pwmMin, - const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const rcMode_t & rcMode, const bool & reverse, const float & scale) : - AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), - _pwmMin(this, 2, pwmMin, PSTR("pMin")), - _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), - _pwmMax(this, 4, pwmMax, PSTR("pMax")), - _reverse(this, 5, reverse, PSTR("rev")), - _scale(scale == 0 ? AP_Float(0) : AP_Float(this,6,reverse,PSTR("scale"))), - _rcMode(rcMode), _rc(rc), _pwm(pwmNeutral) { - //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); - if (rcMode == RC_MODE_IN) - return; - //Serial.print("pwm set for ch: "); Serial.println(int(ch)); - rc->OutputCh(ch, pwmNeutral); - -} - -uint16_t AP_RcChannel::getRadioPwm() { - if (_rcMode == RC_MODE_OUT) { - Serial.print("tryng to read from output channel: "); - Serial.println(int(_ch)); - return _pwmNeutral; // if this happens give a safe value of neutral - } - return _rc->InputCh(_ch); -} - -void AP_RcChannel::setPwm(uint16_t pwm) { - //Serial.printf("pwm in setPwm: %d\n", pwm); - //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); - - // apply reverse - if (_reverse) - pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; - //Serial.printf("pwm after reverse: %d\n", pwm); - - // apply saturation - if (_pwm > uint8_t(_pwmMax)) - _pwm = _pwmMax; - if (_pwm < uint8_t(_pwmMin)) - _pwm = _pwmMin; - _pwm = pwm; - - //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); - if (_rcMode == RC_MODE_IN) - return; - _rc->OutputCh(_ch, _pwm); -} - -uint16_t AP_RcChannel::_positionToPwm(const float & position) { - uint16_t pwm; - //Serial.printf("position: %f\n", position); - if (position < 0) - pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; - else - pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; - - if (pwm > uint16_t(_pwmMax)) - pwm = _pwmMax; - if (pwm < uint16_t(_pwmMin)) - pwm = _pwmMin; - return pwm; -} - -float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { - float position; - // note a piece-wise linear mapping occurs if the pwm ranges - // are not symmetric about pwmNeutral - if (pwm < uint8_t(_pwmNeutral)) - position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( - _pwmNeutral - _pwmMin); - else - position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( - _pwmMax - _pwmNeutral); - if (position > 1) - position = 1; - if (position < -1) - position = -1; - return position; -} - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h deleted file mode 100644 index f9c8138310..0000000000 --- a/libraries/APO/AP_RcChannel.h +++ /dev/null @@ -1,85 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_RcChannel.h -/// @brief AP_RcChannel manager - -#ifndef AP_RCCHANNEL_H -#define AP_RCCHANNEL_H - -#include -#include "../APM_RC/APM_RC.h" -#include "../AP_Common/AP_Common.h" -#include "../AP_Common/AP_Var.h" - -namespace apo { - -enum rcMode_t { - RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT -}; - -/// @class AP_RcChannel -/// @brief Object managing one RC channel -class AP_RcChannel: public AP_Var_group { - -public: - - /// Constructor - AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class * rc, - const uint8_t & ch, const uint16_t & pwmMin, - const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const rcMode_t & rcMode, - const bool & reverse, const float & scale = 0); - - // configuration - AP_Uint8 _ch; - AP_Uint16 _pwmMin; - AP_Uint16 _pwmNeutral; - AP_Uint16 _pwmMax; - rcMode_t _rcMode; - AP_Bool _reverse; - AP_Float _scale; - - // get - uint16_t getPwm() { - return _pwm; - } - uint16_t getRadioPwm(); - float getPosition() { - return _pwmToPosition(getPwm()); - } - float getRadioPosition() { - return _pwmToPosition(getRadioPwm()); - } - float getScaled() { - return _scale*getPwm(); - } - - // set - void setUsingRadio() { - if (_rcMode != RC_MODE_OUT) setPwm(getRadioPwm()); - } - void setPwm(uint16_t pwm); - void setPosition(float position) { - setPwm(_positionToPwm(position)); - } - void setScaled(float val) { - setPwm(val/_scale); - } - -protected: - - // configuration - APM_RC_Class * _rc; - - // internal states - uint16_t _pwm; // this is the internal state, position is just created when needed - - // private methods - uint16_t _positionToPwm(const float & position); - float _pwmToPosition(const uint16_t & pwm); -}; - -} // apo - -#endif // AP_RCCHANNEL_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h deleted file mode 100644 index f9c7c6dbb5..0000000000 --- a/libraries/APO/AP_Var_keys.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef AP_Var_keys_H -#define AP_Var_keys_H - -enum keys { - - // general - k_config = 0, - k_cntrl, - k_guide, - k_sensorCalib, - k_camera, - k_nav, - - k_radioChannelsStart=10, - - k_controllersStart=30, - - k_customStart=100, - - // 200-256 reserved for commands - k_commands = 200 -}; - -// max 256 keys - -#endif -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1.cpp b/libraries/APO/Board_APM1.cpp deleted file mode 100644 index c6c9810513..0000000000 --- a/libraries/APO/Board_APM1.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/* - * Board_APM1.cpp - * - * Created on: Dec 7, 2011 - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "Board_APM1.h" - -namespace apo { - -Board_APM1::Board_APM1(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Board(mode,vehicle,options) { - - const uint32_t debugBaud = 57600; - const uint32_t telemBaud = 57600; - const uint32_t gpsBaud = 38400; - const uint32_t hilBaud = 115200; - const uint8_t batteryPin = 0; - const float batteryVoltageDivRatio = 6; - const float batteryMinVolt = 10.0; - const float batteryMaxVolt = 12.4; - Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; - - AP_Var::load_all(); - - Wire.begin(); - - // debug - Serial.begin(debugBaud, 128, 128); - debug = &Serial; - debug->println_P(PSTR("initialized debug port")); - - // gcs - Serial3.begin(telemBaud, 128, 128); - gcsPort = &Serial3; - gcsPort->println_P(PSTR("initialized gcs port")); - - // hil - Serial1.begin(hilBaud, 128, 128); - hilPort = &Serial1; - hilPort->println_P(PSTR("initialized hil port")); - - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - - eepromMaxAddr = 1024; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - isr_registry = new Arduino_Mega_ISR_Registry; - radio = new APM_RC_APM1; - radio->Init(isr_registry); - dataFlash = new DataFlash_APM1; - scheduler = new AP_TimerProcess; - scheduler->init(isr_registry); - adc = new AP_ADC_ADS7844; - adc->Init(scheduler); - - /* - * Sensor initialization - */ - if (getMode() == MODE_LIVE) { - - if (_options & opt_batteryMonitor) { - batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); - } - - if (_options & opt_gps) { - Serial1.begin(gpsBaud, 128, 16); // gps - debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1, &(gps)); - gps = &gpsDriver; - gps->callback = delay; - gps->init(); - } - - if (_options & opt_baro) { - debug->println_P(PSTR("initializing baro")); - baro = new APM_BMP085_Class; - baro->Init(0,false); - } - - if (_options & opt_compass) { - debug->println_P(PSTR("initializing compass")); - compass = new AP_Compass_HMC5843; - compass->set_orientation(compassOrientation); - compass->set_offsets(0,0,0); - compass->set_declination(0.0); - compass->init(); - } - } - - /** - * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not - * initialize them and NULL will be assigned to those corresponding pointers. - * On detecting NU/LL assigned to any ultrasonic sensor, its corresponding block of code - * will not be executed by the navigator. - * The coordinate system is assigned by the right hand rule with the thumb pointing down. - * In set_orientation, it is defined as (front/back,left/right,down,up) - */ - - // XXX this isn't really that general, should be a better way - - if (_options & opt_rangeFinderFront) { - debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(1),new ModeFilter); - rangeFinder->set_orientation(1, 0, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderBack) { - debug->println_P(PSTR("initializing back range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(2),new ModeFilter); - rangeFinder->set_orientation(-1, 0, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderLeft) { - debug->println_P(PSTR("initializing left range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(3),new ModeFilter); - rangeFinder->set_orientation(0, -1, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderRight) { - debug->println_P(PSTR("initializing right range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(4),new ModeFilter); - rangeFinder->set_orientation(0, 1, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderUp) { - debug->println_P(PSTR("initializing up range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(5),new ModeFilter); - rangeFinder->set_orientation(0, 0, -1); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderDown) { - debug->println_P(PSTR("initializing down range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(6),new ModeFilter); - rangeFinder->set_orientation(0, 0, 1); - rangeFinders.push_back(rangeFinder); - } - - /* - * navigation sensors - */ - debug->println_P(PSTR("initializing imu")); - ins = new AP_InertialSensor_Oilpan(adc); - ins->init(scheduler); - //ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin) - debug->println_P(PSTR("initializing ins")); - imu = new AP_IMU_INS(ins, k_sensorCalib); - imu->init(IMU::WARM_START,delay,scheduler); - debug->println_P(PSTR("setup completed")); -} - -} // namespace apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1.h b/libraries/APO/Board_APM1.h deleted file mode 100644 index 506ce310d7..0000000000 --- a/libraries/APO/Board_APM1.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Board_APM1.h - * - * Created on: Dec 7, 2011 - * - */ - -#ifndef Board_APM1_H_ -#define Board_APM1_H_ - -#include "AP_Board.h" - -namespace apo { - -class Board_APM1 : public AP_Board { -public: - Board_APM1(mode_e mode, MAV_TYPE vehicle, options_t options); -private: -}; - -} // namespace apo - -#endif /* AP_BOARD_APM1_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1_2560.cpp b/libraries/APO/Board_APM1_2560.cpp deleted file mode 100644 index 6fa0e9c595..0000000000 --- a/libraries/APO/Board_APM1_2560.cpp +++ /dev/null @@ -1,14 +0,0 @@ -/* - * Board_APM1_2560.cpp - * - * Created on: Dec 7, 2011 - * - */ - -#include "Board_APM1_2560.h" - -namespace apo { - -} // namespace apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM1_2560.h b/libraries/APO/Board_APM1_2560.h deleted file mode 100644 index 6465d93b63..0000000000 --- a/libraries/APO/Board_APM1_2560.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Board_APM1_2560.h - * - * Created on: Dec 7, 2011 - * - */ - -#ifndef Board_APM1_2560_H_ -#define Board_APM1_2560_H_ - -#include "Board_APM1.h" - -namespace apo { - -class Board_APM1_2560 : public Board_APM1 { -public: - Board_APM1_2560(AP_Board::mode_e mode, MAV_TYPE vehicle, options_t options) : Board_APM1(mode,vehicle,options) { - eepromMaxAddr = 1024; - } -private: -}; - -} // namespace apo - -#endif /* AP_BOARD_APM1_2560_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM2.cpp b/libraries/APO/Board_APM2.cpp deleted file mode 100644 index 459d1bca82..0000000000 --- a/libraries/APO/Board_APM2.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/* - * Board_APM2.cpp - * - * Created on: Dec 7, 2011 - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "Board_APM2.h" - -namespace apo { - -Board_APM2::Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Board(mode,vehicle,options) { - - const uint32_t debugBaud = 57600; - const uint32_t telemBaud = 57600; - const uint32_t gpsBaud = 38400; - const uint32_t hilBaud = 115200; - const uint8_t batteryPin = 0; - const float batteryVoltageDivRatio = 6; - const float batteryMinVolt = 10.0; - const float batteryMaxVolt = 12.4; - Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; - - AP_Var::load_all(); - - Wire.begin(); - - // debug - Serial.begin(debugBaud, 128, 128); - debug = &Serial; - debug->println_P(PSTR("initialized debug port")); - - // gcs - Serial2.begin(telemBaud, 128, 128); - gcsPort = &Serial2; - gcsPort->println_P(PSTR("initialized gcs port")); - delay(1000); - - // hil - Serial1.begin(hilBaud, 128, 128); - hilPort = &Serial1; - hilPort->println_P(PSTR("initialized hil port")); - delay(1000); - - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - - eepromMaxAddr = 1024; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - isr_registry = new Arduino_Mega_ISR_Registry; - radio = new APM_RC_APM2; - radio->Init(isr_registry); - dataFlash = new DataFlash_APM2; - scheduler = new AP_TimerProcess; - scheduler->init(isr_registry); - adc = new AP_ADC_ADS7844; - adc->Init(scheduler); - - /* - * Sensor initialization - */ - if (getMode() == MODE_LIVE) { - - if (_options & opt_batteryMonitor) { - batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); - } - - if (_options & opt_gps) { - Serial1.begin(gpsBaud, 128, 16); // gps - debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1, &(gps)); - gps = &gpsDriver; - gps->callback = delay; - gps->init(); - } - - if (_options & opt_baro) { - debug->println_P(PSTR("initializing baro")); - baro = new APM_BMP085_Class; - baro->Init(0,true); - } - - if (_options & opt_compass) { - debug->println_P(PSTR("initializing compass")); - compass = new AP_Compass_HMC5843; - compass->set_orientation(compassOrientation); - compass->set_offsets(0,0,0); - compass->set_declination(0.0); - compass->init(); - } - } - - /** - * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not - * initialize them and NULL will be assigned to those corresponding pointers. - * On detecting NU/LL assigned to any ultrasonic sensor, its corresponding block of code - * will not be executed by the navigator. - * The coordinate system is assigned by the right hand rule with the thumb pointing down. - * In set_orientation, it is defined as (front/back,left/right,down,up) - */ - - // XXX this isn't really that general, should be a better way - - if (_options & opt_rangeFinderFront) { - debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(1),new ModeFilter); - rangeFinder->set_orientation(1, 0, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderBack) { - debug->println_P(PSTR("initializing back range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(2),new ModeFilter); - rangeFinder->set_orientation(-1, 0, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderLeft) { - debug->println_P(PSTR("initializing left range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(3),new ModeFilter); - rangeFinder->set_orientation(0, -1, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderRight) { - debug->println_P(PSTR("initializing right range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(4),new ModeFilter); - rangeFinder->set_orientation(0, 1, 0); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderUp) { - debug->println_P(PSTR("initializing up range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(5),new ModeFilter); - rangeFinder->set_orientation(0, 0, -1); - rangeFinders.push_back(rangeFinder); - } - - if (_options & opt_rangeFinderDown) { - debug->println_P(PSTR("initializing down range finder")); - RangeFinder * rangeFinder = new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(6),new ModeFilter); - rangeFinder->set_orientation(0, 0, 1); - rangeFinders.push_back(rangeFinder); - } - - /* - * navigation sensors - */ - debug->println_P(PSTR("initializing imu")); - ins = new AP_InertialSensor_MPU6000(53); - ins->init(scheduler); - debug->println_P(PSTR("initializing ins")); - imu = new AP_IMU_INS(ins, k_sensorCalib); - imu->init(IMU::WARM_START,delay,scheduler); - debug->println_P(PSTR("setup completed")); -} - -} // namespace apo - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Board_APM2.h b/libraries/APO/Board_APM2.h deleted file mode 100644 index b14e6226e7..0000000000 --- a/libraries/APO/Board_APM2.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Board_APM2.h - * - * Created on: Dec 7, 2011 - * - */ - -#ifndef Board_APM2_H_ -#define Board_APM2_H_ - -#include "AP_Board.h" - -namespace apo { - -class Board_APM2 : public AP_Board { -public: - Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options); -private: -}; - -} // namespace apo - -#endif /* AP_BOARD_APM2_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Navigator_Dcm.cpp b/libraries/APO/Navigator_Dcm.cpp deleted file mode 100644 index a7026b17e8..0000000000 --- a/libraries/APO/Navigator_Dcm.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* - * Navigator_Dcm.cpp - * - * Created on: Dec 6, 2011 - * Author: jgoppert/ wenyaoxie - */ - -#include "../FastSerial/FastSerial.h" -#include "Navigator_Dcm.h" -#include "AP_CommLink.h" -#include "AP_Board.h" -#include "../AP_DCM/AP_DCM.h" -#include "../AP_Math/AP_Math.h" -#include "../AP_Compass/AP_Compass.h" -#include "AP_MavlinkCommand.h" -#include "AP_Var_keys.h" -#include "../AP_RangeFinder/RangeFinder.h" -#include "../AP_IMU/AP_IMU.h" -#include "../AP_InertialSensor/AP_InertialSensor.h" -#include "../APM_BMP085/APM_BMP085_hil.h" -#include "../APM_BMP085/APM_BMP085.h" - -namespace apo { - -Navigator_Dcm::Navigator_Dcm(AP_Board * board, const uint16_t key, const prog_char_t * name) : - AP_Navigator(board), _imuOffsetAddress(0), - _dcm(_board->imu, _board->gps, _board->compass), - _rangeFinderDown(), - _group(key, name ? : PSTR("NAV_")), - _baroLowPass(&_group,1,10,PSTR("BAROLP")), - _groundTemperature(&_group,2, 25.0,PSTR("GNDT")), // TODO check temp - _groundPressure(&_group,3,0.0,PSTR("GNDP")) { - - // if orientation equal to front, store as front - /** - * rangeFinder is assigned values based on orientation which - * is specified in ArduPilotOne.pde. - */ - for (uint8_t i = 0; i < _board-> rangeFinders.getSize(); i++) { - if (_board->rangeFinders[i] == NULL) - continue; - if (_board->rangeFinders[i]->orientation_x == 0 - && _board->rangeFinders[i]->orientation_y == 0 - && _board->rangeFinders[i]->orientation_z == 1) - _rangeFinderDown = _board->rangeFinders[i]; - } - - // tune down dcm - _dcm.kp_roll_pitch(0.030000); - _dcm.ki_roll_pitch(0.00001278), // 50 hz I term - - // tune down compass in dcm - _dcm.kp_yaw(0.08); - _dcm.ki_yaw(0); - - if (_board->compass) { - _dcm.set_compass(_board->compass); - } -} -void Navigator_Dcm::calibrate() { - - AP_Navigator::calibrate(); - - // TODO: handle cold/warm restart - if (_board->imu) { - _board->imu->init(IMU::COLD_START,delay,_board->scheduler); - } - - if (_board->baro) { - - int flashcount = 0; - - while(_groundPressure == 0){ - _board->baro->Read(); // Get initial data from absolute pressure sensor - _groundPressure = _board->baro->Press; - _groundTemperature = _board->baro->Temp/10.0; - delay(20); - } - - for(int i = 0; i < 30; i++){ // We take some readings... - - // set using low pass filters - _groundPressure = _groundPressure * 0.9 + _board->baro->Press * 0.1; - _groundTemperature = _groundTemperature * 0.9 + (_board->baro->Temp/10.0) * 0.1; - - //mavlink_delay(20); - delay(20); - if(flashcount == 5) { - digitalWrite(_board->cLedPin, LOW); - digitalWrite(_board->aLedPin, HIGH); - digitalWrite(_board->bLedPin, LOW); - } - - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(_board->cLedPin, LOW); - digitalWrite(_board->aLedPin, HIGH); - digitalWrite(_board->bLedPin, LOW); - } - flashcount++; - } - - _groundPressure.save(); - _groundTemperature.save(); - - _board->debug->printf_P(PSTR("ground pressure: %ld ground temperature: %d\n"),_groundPressure.get(), _groundTemperature.get()); - _board->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n")); - } -} - -void Navigator_Dcm::updateFast(float dt) { - - if (_board->getMode() != AP_Board::MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - // use range finder if attached and close to the ground - if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) { - setAlt(_rangeFinderDown->distance); - - // otherwise if you have a baro attached, use it - } else if (_board->baro) { - /** - * The altitued is read off the barometer by implementing the following formula: - * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), - * where, po is pressure in Pa at sea level (101325 Pa). - * See http://www.sparkfun.com/tutorials/253 or type this formula - * in a search engine for more information. - * altInt contains the altitude in meters. - * - * pressure input is in pascals - * temp input is in deg C *10 - */ - _board->baro->Read(); // Get new data from absolute pressure sensor - float reference = 44330.0 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295))); - setAlt(_baroLowPass.update((44330.0 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt)); - //_board->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_board->baro->Press,_board->baro->Temp); - - // last resort, use gps altitude - } else if (_board->gps && _board->gps->fix) { - setAlt_intM(_board->gps->altitude * 10); // gps in cm, intM in mm - } - - // update dcm calculations and navigator data - // - _dcm.update_DCM_fast(); - setRoll(_dcm.roll); - setPitch(_dcm.pitch); - setYaw(_dcm.yaw); - setRollRate(_dcm.get_gyro().x); - setPitchRate(_dcm.get_gyro().y); - setYawRate(_dcm.get_gyro().z); - setXAccel(_dcm.get_accel().x); - setYAccel(_dcm.get_accel().y); - setZAccel(_dcm.get_accel().z); - - /* - * accel/gyro debug - */ - /* - Vector3f accel = _board->imu->get_accel(); - Vector3f gyro = _board->imu->get_gyro(); - Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), - accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); - */ -} - -void Navigator_Dcm::updateSlow(float dt) { - if (_board->getMode() != AP_Board::MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - if (_board->gps) { - _board->gps->update(); - updateGpsLight(); - if (_board->gps->fix && _board->gps->new_data) { - setLat_degInt(_board->gps->latitude); - setLon_degInt(_board->gps->longitude); - setGroundSpeed(_board->gps->ground_speed / 100.0); // gps is in cm/s - } - } - - if (_board->compass) { - _board->compass->read(); - _board->compass->calculate(_dcm.get_dcm_matrix()); - _board->compass->null_offsets(_dcm.get_dcm_matrix()); - //_board->debug->printf_P(PSTR("heading: %f"), _board->compass->heading); - } -} -void Navigator_Dcm::updateGpsLight(void) { - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - static bool GPS_light = false; - switch (_board->gps->status()) { - case (2): - //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case (1): - if (_board->gps->valid_read == true) { - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light) { - digitalWrite(_board->cLedPin, LOW); - } else { - digitalWrite(_board->cLedPin, HIGH); - } - _board->gps->valid_read = false; - } - break; - - default: - digitalWrite(_board->cLedPin, LOW); - break; - } -} - -} // namespace apo -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/Navigator_Dcm.h b/libraries/APO/Navigator_Dcm.h deleted file mode 100644 index 015e6624e5..0000000000 --- a/libraries/APO/Navigator_Dcm.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Navigator_Dcm.h - * Copyright (C) James Goppert/ Wenyao Xie 2011 james.goppert@gmail.com/ wenyaoxie@gmail.com - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef Navigator_Dcm_H -#define Navigator_Dcm_H - -#include "AP_Navigator.h" -#include -#include "AP_ControllerBlock.h" -#include - -class RangeFinder; - -namespace apo { - -class Navigator_Dcm: public AP_Navigator { -public: - Navigator_Dcm(AP_Board * board, const uint16_t key, const prog_char_t * name = NULL); - virtual void calibrate(); - virtual void updateFast(float dt); - virtual void updateSlow(float dt); - void updateGpsLight(void); -private: - AP_DCM _dcm; - AP_Var_group _group; - uint16_t _imuOffsetAddress; - BlockLowPass _baroLowPass; - AP_Float _groundTemperature; - AP_Float _groundPressure; - RangeFinder * _rangeFinderDown; -}; - -} // namespace apo - -#endif // Navigator_Dcm_H -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/constants.h b/libraries/APO/constants.h deleted file mode 100644 index 27f03c50b8..0000000000 --- a/libraries/APO/constants.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * constants.h - * - * Created on: Apr 7, 2011 - * Author: nkgentry - */ - -#ifndef CONSTANTS_H_ -#define CONSTANTS_H_ - -#include "math.h" - -const float scale_deg = 1e7; // scale of integer degrees/ degree -const float scale_m = 1e3; // scale of integer meters/ meter -const float rEarth = 6371000; // radius of earth, meters -const float rad2Deg = 180 / M_PI; // radians to degrees -const float deg2Rad = M_PI / 180; // degrees to radians -const float rad2DegInt = rad2Deg * scale_deg; // radians to degrees x 1e7 -const float degInt2Rad = deg2Rad / scale_deg; // degrees x 1e7 to radians - -#define MAV_MODE_FAILSAFE MAV_MODE_TEST3 - -#endif /* CONSTANTS_H_ */ -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/examples/MavlinkTest/Makefile b/libraries/APO/examples/MavlinkTest/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/MavlinkTest/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde deleted file mode 100644 index f45cd2e510..0000000000 --- a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde +++ /dev/null @@ -1,49 +0,0 @@ -/* - AnalogReadSerial - Reads an analog input on pin 0, prints the result to the serial monitor - - This example code is in the public domain. - */ - -#include - -int packetDrops = 0; - -void handleMessage(mavlink_message_t * msg) { - Serial.print(", received mavlink message: "); - Serial.print(msg->msgid, DEC); -} - -void setup() { - Serial.begin(57600); - Serial3.begin(57600); - mavlink_comm_0_port = &Serial3; - packetDrops = 0; -} - -void loop() { - mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - Serial.print("heartbeat sent"); - - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - - Serial.print(", bytes available: "); - Serial.print(comm_get_available(MAVLINK_COMM_0)); - while (comm_get_available( MAVLINK_COMM_0)) { - uint8_t c = comm_receive_ch(MAVLINK_COMM_0); - - // Try to get a new message - if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) - handleMessage(&msg); - } - - // Update packet drops counter - packetDrops += status.packet_rx_drop_count; - - Serial.print(", dropped packets: "); - Serial.println(packetDrops); - delay(1000); -} diff --git a/libraries/APO/examples/ServoManual/Makefile b/libraries/APO/examples/ServoManual/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/ServoManual/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoManual/ServoManual.pde b/libraries/APO/examples/ServoManual/ServoManual.pde deleted file mode 100644 index 7e07d3e153..0000000000 --- a/libraries/APO/examples/ServoManual/ServoManual.pde +++ /dev/null @@ -1,112 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - - */ -#include -#include -#include -#include // ArduPilot Mega RC Library -#include -#include -#include -FastSerialPort0(Serial) -; // make sure this proceeds variable declarations - -// test settings - -using namespace apo; - -class RadioTest { -private: - float testPosition; - int8_t testSign; - enum { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; - APM_RC_APM1 radio; - Arduino_Mega_ISR_Registry isr_registry; -public: - RadioTest() : - testPosition(2), testSign(1) { - ch.push_back( - new AP_RcChannel(rollKey, PSTR("ROLL"), &radio, 0, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(pitchKey, PSTR("PITCH"), &radio, 1, 1100, - 1500, 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(thrKey, PSTR("THR"), &radio, 2, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(yawKey, PSTR("YAW"), &radio, 3, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(ch5Key, PSTR("CH5"), &radio, 4, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(ch6Key, PSTR("CH6"), &radio, 5, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(ch7Key, PSTR("CH7"), &radio, 6, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - ch.push_back( - new AP_RcChannel(ch8Key, PSTR("CH8"), &radio, 7, 1100, 1500, - 1900, RC_MODE_INOUT, false)); - radio.Init(&isr_registry); - - Serial.begin(57600); - delay(2000); - Serial.println("ArduPilot RC Channel test"); - delay(2000); - } - - void update() { - // read the radio - for (uint8_t i = 0; i < ch.getSize(); i++) - ch[i]->setUsingRadio(); - - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (uint8_t i = 0; i < ch.getSize(); i++) { - ch[i]->copy_name(name, 7); - Serial.printf("%7s\t", name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7d\t", ch[i]->getPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7.2f\t", ch[i]->getPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() { - test = new RadioTest; -} - -void loop() { - test->update(); -} diff --git a/libraries/APO/examples/ServoSweep/Makefile b/libraries/APO/examples/ServoSweep/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/ServoSweep/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde deleted file mode 100644 index fa6f7b746e..0000000000 --- a/libraries/APO/examples/ServoSweep/ServoSweep.pde +++ /dev/null @@ -1,125 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - - */ -#include -#include -#include -#include // ArduPilot Mega RC Library -#include -#include -FastSerialPort0(Serial) -; // make sure this proceeds variable declarations - -// test settings - -using namespace apo; - -class RadioTest { -private: - float testPosition; - int8_t testSign; - enum { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; -public: - RadioTest() : - testPosition(2), testSign(1) { - ch.push_back( - new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, - 1500, 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - ch.push_back( - new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, - 1900,RC_MODE_INOUT,false)); - - Serial.begin(115200); - delay(2000); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } - - void update() { - // update test value - testPosition += testSign * .1; - if (testPosition > 1) { - //eepromRegistry.print(Serial); // show eeprom map - testPosition = 1; - testSign = -1; - } else if (testPosition < -1) { - testPosition = -1; - testSign = 1; - } - - // set channel positions - for (uint8_t i = 0; i < ch.getSize(); i++) - ch[i]->setPosition(testPosition); - - // print test position - Serial.printf("\nnormalized position (%f)\n", testPosition); - - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (uint8_t i = 0; i < ch.getSize(); i++) { - ch[i]->copy_name(name, 7); - Serial.printf("%7s\t", name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7d\t", ch[i]->getRadioPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7.2f\t", ch[i]->getRadioPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() { - test = new RadioTest; -} - -void loop() { - test->update(); -} - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/template.h b/libraries/APO/template.h deleted file mode 100644 index 765d1fe5bb..0000000000 --- a/libraries/APO/template.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Class.h - * Copyright (C) Author 2011 - * - * This file is free software: you can redistribute it and/or modify it - * under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program. If not, see . - */ - -#ifndef Class_H -#define Class_H - -namespace apo { - -/// Class description -class Class { -public: -}; - -} // namespace apo - -#endif // Class_H -// vim:ts=4:sw=4:expandtab