aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove APO library: deprecated

Browse Source
This commit is contained in:
Pat Hickey 2012-10-26 19:48:03 -07:00 committed by Andrew Tridgell
parent 68ee5fa0f4
commit d50d0682e9
51 changed files with 0 additions and 5769 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
apo/.gitignore vendored
View File

@ -1 +0,0 @@
apo.cpp

164
apo/ControllerPlane.h
View File

@ -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

192
apo/ControllerQuad.h
View File

@ -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

1
apo/Makefile
View File

@ -1 +0,0 @@
include ../libraries/AP_Common/Arduino.mk

102
apo/PlaneEasystar.h
View File

@ -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

109
apo/QuadArducopter.h
View File

@ -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

24
apo/apo.pde
View File

@ -1,24 +0,0 @@
// Libraries
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <AP_AnalogSource.h>
#include <AP_InertialSensor.h>
// Vehicle Configuration
#include "QuadArducopter.h"
//#include "PlaneEasystar.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"

8
libraries/APO/APO.cpp
View File

@ -1,8 +0,0 @@
/*
* APO.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#include "APO.h"

34
libraries/APO/APO.h
View File

@ -1,34 +0,0 @@
/*
* APO.h
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#ifndef APO_H_
#define APO_H_
#include <FastSerial.h>
#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

46
libraries/APO/APO_DefaultSetup.h
View File

@ -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

55
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -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

69
libraries/APO/AP_ArmingMechanism.h
View File

@ -1,69 +0,0 @@
/*
* AP_ArmingMechanism.h
*
*/
#ifndef AP_ARMINGMECHANISM_H_
#define AP_ARMINGMECHANISM_H_
#include <inttypes.h>
#include <wiring.h>
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

273
libraries/APO/AP_Autopilot.cpp
View File

@ -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

123
libraries/APO/AP_Autopilot.h
View File

@ -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

12
libraries/APO/AP_BatteryMonitor.cpp
View File

@ -1,12 +0,0 @@
/*
* AP_BatteryMonitor.cpp
*
*/
#include "AP_BatteryMonitor.h"
namespace apo {
} // apo
// vim:ts=4:sw=4:expandtab

52
libraries/APO/AP_BatteryMonitor.h
View File

@ -1,52 +0,0 @@
/*
* AP_BatteryMonitor.h
*
*/
#ifndef AP_BATTERYMONITOR_H_
#define AP_BATTERYMONITOR_H_
#include <inttypes.h>
#include <wiring.h>
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

14
libraries/APO/AP_Board.cpp
View File

@ -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

139
libraries/APO/AP_Board.h
View File

@ -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<RangeFinder *> 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<AP_RcChannel *> 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

800
libraries/APO/AP_CommLink.cpp
View File

@ -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

141
libraries/APO/AP_CommLink.h
View File

@ -1,141 +0,0 @@
/*
* AP_CommLink.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#ifndef AP_CommLink_H
#define AP_CommLink_H
#include <inttypes.h>
#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<mavlink_command_t *> _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

106
libraries/APO/AP_Controller.cpp
View File

@ -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

148
libraries/APO/AP_Controller.h
View File

@ -1,148 +0,0 @@
/*
* AP_Controller.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#ifndef AP_Controller_H
#define AP_Controller_H
// inclusions
#include "../AP_Common/AP_Common.h"
#include "../AP_Common/AP_Var.h"
#include <inttypes.h>
#include <math.h>
#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

176
libraries/APO/AP_ControllerBlock.cpp
View File

@ -1,176 +0,0 @@
/*
* AP_ControllerBlock.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#include "AP_ControllerBlock.h"
#include <math.h>
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

235
libraries/APO/AP_ControllerBlock.h
View File

@ -1,235 +0,0 @@
/*
* AP_ControllerBlock.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#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

236
libraries/APO/AP_Guide.cpp
View File

@ -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

167
libraries/APO/AP_Guide.h
View File

@ -1,167 +0,0 @@
/*
* AP_Guide.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#ifndef AP_Guide_H
#define AP_Guide_H
#include <inttypes.h>
#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

52
libraries/APO/AP_Loop.cpp
View File

@ -1,52 +0,0 @@
/*
* AP_Loop.pde
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#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

61
libraries/APO/AP_Loop.h
View File

@ -1,61 +0,0 @@
/*
* AP_Loop.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#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<Loop *> & 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<Loop *> _subLoops;
uint32_t _timeStamp;
uint8_t _load;
float _dt;
};
#endif
// vim:ts=4:sw=4:expandtab

212
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -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

380
libraries/APO/AP_MavlinkCommand.h
View File

@ -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<CommandStorage> _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

45
libraries/APO/AP_Navigator.cpp
View File

@ -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

297
libraries/APO/AP_Navigator.h
View File

@ -1,297 +0,0 @@
/*
* AP_Navigator.h
* Copyright (C) James Goppert 2010 <james.goppert@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 <http://www.gnu.org/licenses/>.
*/
#ifndef AP_Navigator_H
#define AP_Navigator_H
#include "constants.h"
#include <inttypes.h>
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

103
libraries/APO/AP_RcChannel.cpp
View File

@ -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 <math.h>
#include <avr/eeprom.h>
#include "AP_RcChannel.h"
#include "../AP_Common/AP_Common.h"
#include <HardwareSerial.h>
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

85
libraries/APO/AP_RcChannel.h
View File

@ -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 <stdint.h>
#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

27
libraries/APO/AP_Var_keys.h
View File

@ -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

186
libraries/APO/Board_APM1.cpp
View File

@ -1,186 +0,0 @@
/*
* Board_APM1.cpp
*
* Created on: Dec 7, 2011
*
*/
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <AP_AnalogSource.h>
#include <AP_InertialSensor.h>
#include <DataFlash.h>
#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

24
libraries/APO/Board_APM1.h
View File

@ -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

14
libraries/APO/Board_APM1_2560.cpp
View File

@ -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

26
libraries/APO/Board_APM1_2560.h
View File

@ -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

187
libraries/APO/Board_APM2.cpp
View File

@ -1,187 +0,0 @@
/*
* Board_APM2.cpp
*
* Created on: Dec 7, 2011
*
*/
#include <Wire.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h>
#include <AP_RangeFinder.h>
#include <GCS_MAVLink.h>
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <AP_AnalogSource.h>
#include <AP_InertialSensor.h>
#include <DataFlash.h>
#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

24
libraries/APO/Board_APM2.h
View File

@ -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

220
libraries/APO/Navigator_Dcm.cpp
View File

@ -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<direction> 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

51
libraries/APO/Navigator_Dcm.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#ifndef Navigator_Dcm_H
#define Navigator_Dcm_H
#include "AP_Navigator.h"
#include <FastSerial.h>
#include "AP_ControllerBlock.h"
#include <AP_DCM.h>
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

24
libraries/APO/constants.h
View File

@ -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

2
libraries/APO/examples/MavlinkTest/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

49
libraries/APO/examples/MavlinkTest/MavlinkTest.pde
View File

@ -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 <GCS_MAVLink.h>
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);
}

2
libraries/APO/examples/ServoManual/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

112
libraries/APO/examples/ServoManual/ServoManual.pde
View File

@ -1,112 +0,0 @@
/*
Example of RC_Channel library.
Code by James Goppert/ Jason Short. 2010
DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <APO.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <APM_RC.h>
#include <AP_Vector.h>
#include <Arduino_Mega_ISR_Registry.h>
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<AP_RcChannel *> 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();
}

2
libraries/APO/examples/ServoSweep/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

125
libraries/APO/examples/ServoSweep/ServoSweep.pde
View File

@ -1,125 +0,0 @@
/*
Example of RC_Channel library.
Code by James Goppert/ Jason Short. 2010
DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <APO.h>
#include <AP_RcChannel.h> // ArduPilot Mega RC Library
#include <APM_RC.h>
#include <AP_Vector.h>
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<AP_RcChannel *> 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

32
libraries/APO/template.h
View File

@ -1,32 +0,0 @@
/*
* Class.h
* Copyright (C) Author 2011 <email>
*
* 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 <http://www.gnu.org/licenses/>.
*/
#ifndef Class_H
#define Class_H
namespace apo {
/// Class description
class Class {
public:
};
} // namespace apo
#endif // Class_H
// vim:ts=4:sw=4:expandtab