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

APO formatting.

This commit is contained in:
James Goppert 2011-10-26 12:31:11 -04:00
parent 6cd604e510
commit 18e8bb3d6a
26 changed files with 3025 additions and 3004 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

308
apo/ControllerPlane.h
View File

@ -14,180 +14,180 @@ namespace apo {
class ControllerPlane: public AP_Controller {
public:
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,
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_pidBnkRll = k_controllersStart,
k_pidSpdPit,
k_pidPitPit,
k_pidYwrYaw,
k_pidHdgBnk,
k_pidAltThr,
k_trim = k_customStart
};
k_trim = k_customStart
};
ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
_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) {
ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
_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) {
_hal->debug->println_P(PSTR("initializing plane controller"));
_hal->debug->println_P(PSTR("initializing plane controller"));
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200,
1500, 1800, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200,
1500, 1800, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500,
1800, RC_MODE_INOUT, false));
}
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200,
1500, 1800, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200,
1500, 1800, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100,
1900, RC_MODE_INOUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500,
1800, RC_MODE_INOUT, false));
}
void manualLoop(const float dt) {
setAllRadioChannelsManually();
void manualLoop(const float dt) {
setAllRadioChannelsManually();
// force auto to read new manual trim
if (_needsTrim == false)
_needsTrim = true;
//_hal->debug->println("manual");
}
// force auto to read new manual trim
if (_needsTrim == false)
_needsTrim = true;
//_hal->debug->println("manual");
}
void autoLoop(const float dt) {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
void autoLoop(const float dt) {
float headingError = _guide->getHeadingCommand()
- _nav->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
_aileron = pidBnkRll.update(
pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt);
_elevator = pidPitPit.update(
-pidSpdPit.update(
_guide->getAirSpeedCommand() - _nav->getAirSpeed(),
dt) - _nav->getPitch(), dt);
_rudder = pidYwrYaw.update(-_nav->getYawRate(), dt);
_aileron = pidBnkRll.update(
pidHdgBnk.update(headingError, 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);
// desired yaw rate is zero, needs washout
_throttle = pidAltThr.update(
_guide->getAltitudeCommand() - _nav->getAlt(), dt);
// if needs trim
if (_needsTrim) {
// need to subtract current controller deflections so control
// surfaces are actually at the same position as manual flight
_ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron;
_elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator;
_rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder;
_thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle;
_needsTrim = false;
}
// if needs trim
if (_needsTrim) {
// need to subtract current controller deflections so control
// surfaces are actually at the same position as manual flight
_ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron;
_elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator;
_rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder;
_thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle;
_needsTrim = false;
}
// actuator mixing/ output
_aileron += _rdrAilMix * _rudder + _ailTrim;
_elevator += _elvTrim;
_rudder += _rdrTrim;
_throttle += _thrTrim;
// actuator mixing/ output
_aileron += _rdrAilMix * _rudder + _ailTrim;
_elevator += _elvTrim;
_rudder += _rdrTrim;
_throttle += _thrTrim;
//_hal->debug->println("automode");
}
//_hal->debug->println("automode");
}
void setMotors() {
void setMotors() {
switch (_hal->getState()) {
switch (_hal->getState()) {
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
// actuator mixing/ output
_hal->rc[ch_roll]->setPosition(_aileron);
_hal->rc[ch_yaw]->setPosition(_rudder);
_hal->rc[ch_pitch]->setPosition(_elevator);
_hal->rc[ch_thrust]->setPosition(_throttle);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
// actuator mixing/ output
_hal->rc[ch_roll]->setPosition(_aileron);
_hal->rc[ch_yaw]->setPosition(_rudder);
_hal->rc[ch_pitch]->setPosition(_elevator);
_hal->rc[ch_thrust]->setPosition(_throttle);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
}
}
}
}
private:
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;
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

344
apo/ControllerQuad.h
View File

@ -17,199 +17,199 @@ namespace apo {
class ControllerQuad: public AP_Controller {
public:
/**
* note that these are not the controller radio channel numbers, they are just
* unique keys so they can be reaccessed from the hal 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
};
/**
* note that these are not the controller radio channel numbers, they are just
* unique keys so they can be reaccessed from the hal 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
};
// 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_pidPN,
k_pidPE,
k_pidPD,
k_pidRoll,
k_pidPitch,
k_pidYawRate,
k_pidYaw,
};
enum {
k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr,
k_pidPN,
k_pidPE,
k_pidPD,
k_pidRoll,
k_pidPitch,
k_pidYawRate,
k_pidYaw,
};
ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
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, PID_ATT_DFCUT),
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, PID_ATT_DFCUT),
pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
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, PID_ATT_DFCUT),
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, PID_ATT_DFCUT),
pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
/*
* allocate radio channels
* the order of the channels has to match the enumeration above
*/
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100,
1100, 1900, RC_MODE_OUT, false));
/*
* allocate radio channels
* the order of the channels has to match the enumeration above
*/
_hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100,
1100, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500,
1900, RC_MODE_IN, false));
}
_hal->rc.push_back(
new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100,
1100, 1900, RC_MODE_OUT, false));
_hal->rc.push_back(
new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100,
1500, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100,
1100, 1900, RC_MODE_IN, false));
_hal->rc.push_back(
new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500,
1900, RC_MODE_IN, false));
}
private:
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition();
_cmdPitch = -0.5 * _hal->rc[ch_pitch]->getPosition();
_cmdYawRate = -1 * _hal->rc[ch_yaw]->getPosition();
_thrustMix = _hal->rc[ch_thrust]->getPosition();
autoAttitudeLoop(dt);
}
void manualLoop(const float dt) {
setAllRadioChannelsManually();
_cmdRoll = -0.5 * _hal->rc[ch_roll]->getPosition();
_cmdPitch = -0.5 * _hal->rc[ch_pitch]->getPosition();
_cmdYawRate = -1 * _hal->rc[ch_yaw]->getPosition();
_thrustMix = _hal->rc[ch_thrust]->getPosition();
autoAttitudeLoop(dt);
}
void autoLoop(const float dt) {
autoPositionLoop(dt);
autoAttitudeLoop(dt);
void autoLoop(const float dt) {
autoPositionLoop(dt);
autoAttitudeLoop(dt);
// XXX currently auto loop not tested, so
// put vehicle in standby
_hal->setState(MAV_STATE_STANDBY);
}
// XXX currently auto loop not tested, so
// put vehicle in standby
_hal->setState(MAV_STATE_STANDBY);
}
void autoPositionLoop(float dt) {
float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
void autoPositionLoop(float dt) {
float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
// "transform-to-body"
{
float trigSin = sin(-_nav->getYaw());
float trigCos = cos(-_nav->getYaw());
_cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
_cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
// note that the north tilt is negative of the pitch
}
_cmdYawRate = 0;
// "transform-to-body"
{
float trigSin = sin(-_nav->getYaw());
float trigCos = cos(-_nav->getYaw());
_cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
_cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
// note that the north tilt is negative of the pitch
}
_cmdYawRate = 0;
_thrustMix = THRUST_HOVER_OFFSET + cmdDown;
_thrustMix = THRUST_HOVER_OFFSET + cmdDown;
// "thrust-trim-adjust"
if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
else _thrustMix /= cos(_cmdRoll);
// "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);
}
if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
else _thrustMix /= cos(_cmdPitch);
}
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 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() {
void setMotors() {
switch (_hal->getState()) {
switch (_hal->getState()) {
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix);
_hal->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix);
_hal->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix);
_hal->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
case MAV_STATE_ACTIVE: {
digitalWrite(_hal->aLedPin, HIGH);
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
} else {
_hal->rc[ch_right]->setPosition(_thrustMix - _rollMix + _yawMix);
_hal->rc[ch_left]->setPosition(_thrustMix + _rollMix + _yawMix);
_hal->rc[ch_front]->setPosition(_thrustMix + _pitchMix - _yawMix);
_hal->rc[ch_back]->setPosition(_thrustMix - _pitchMix - _yawMix);
}
break;
}
case MAV_STATE_EMERGENCY: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setAllRadioChannelsToNeutral();
break;
}
default: {
digitalWrite(_hal->aLedPin, LOW);
setAllRadioChannelsToNeutral();
}
}
}
}
}
};
} // namespace apo

8
apo/PlaneEasystar.h
View File

@ -27,10 +27,10 @@ static const uint8_t heartBeatTimeout = 3;
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors
static const bool gpsEnabled = false;

8
apo/QuadArducopter.h
View File

@ -27,10 +27,10 @@ static const uint8_t heartBeatTimeout = 3;
#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL
// baud rates
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
static uint32_t debugBaud = 57600;
static uint32_t telemBaud = 57600;
static uint32_t gpsBaud = 38400;
static uint32_t hilBaud = 57600;
// optional sensors
static const bool gpsEnabled = false;

280
libraries/APO/APO_DefaultSetup.h
View File

@ -14,170 +14,170 @@ static apo::AP_Autopilot * autoPilot;
void setup() {
using namespace apo;
AP_Var::load_all();
using namespace apo;
// Declare all parts of the system
AP_Navigator * navigator = NULL;
AP_Guide * guide = NULL;
AP_Controller * controller = NULL;
AP_HardwareAbstractionLayer * hal = NULL;
AP_Var::load_all();
/*
* Communications
*/
Serial.begin(debugBaud, 128, 128); // debug
// hardware abstraction layer
hal = new AP_HardwareAbstractionLayer(
halMode, board, vehicle, heartBeatTimeout);
// debug serial
hal->debug = &Serial;
hal->debug->println_P(PSTR("initializing debug line"));
// Declare all parts of the system
AP_Navigator * navigator = NULL;
AP_Guide * guide = NULL;
AP_Controller * controller = NULL;
AP_HardwareAbstractionLayer * hal = NULL;
/*
* Sensor initialization
*/
if (hal->getMode() == MODE_LIVE) {
/*
* Communications
*/
Serial.begin(debugBaud, 128, 128); // debug
hal->debug->println_P(PSTR("initializing adc"));
hal->adc = new ADC_CLASS;
hal->adc->Init();
// hardware abstraction layer
hal = new AP_HardwareAbstractionLayer(
halMode, board, vehicle, heartBeatTimeout);
if (batteryMonitorEnabled) {
hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt);
}
// debug serial
hal->debug = &Serial;
hal->debug->println_P(PSTR("initializing debug line"));
if (gpsEnabled) {
Serial1.begin(gpsBaud, 128, 16); // gps
hal->debug->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
hal->gps = &gpsDriver;
hal->gps->callback = delay;
hal->gps->init();
}
/*
* Sensor initialization
*/
if (hal->getMode() == MODE_LIVE) {
if (baroEnabled) {
hal->debug->println_P(PSTR("initializing baro"));
hal->baro = new BARO_CLASS;
hal->baro->Init();
}
hal->debug->println_P(PSTR("initializing adc"));
hal->adc = new ADC_CLASS;
hal->adc->Init();
if (compassEnabled) {
Wire.begin();
hal->debug->println_P(PSTR("initializing compass"));
hal->compass = new COMPASS_CLASS;
hal->compass->set_orientation(compassOrientation);
hal->compass->set_offsets(0,0,0);
hal->compass->set_declination(0.0);
hal->compass->init();
}
if (batteryMonitorEnabled) {
hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt);
}
/**
* 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 NULL 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)
*/
if (gpsEnabled) {
Serial1.begin(gpsBaud, 128, 16); // gps
hal->debug->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
hal->gps = &gpsDriver;
hal->gps->callback = delay;
hal->gps->init();
}
if (rangeFinderFrontEnabled) {
hal->debug->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(1);
rangeFinder->set_orientation(1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (baroEnabled) {
hal->debug->println_P(PSTR("initializing baro"));
hal->baro = new BARO_CLASS;
hal->baro->Init();
}
if (rangeFinderBackEnabled) {
hal->debug->println_P(PSTR("initializing back range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(2);
rangeFinder->set_orientation(-1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (compassEnabled) {
Wire.begin();
hal->debug->println_P(PSTR("initializing compass"));
hal->compass = new COMPASS_CLASS;
hal->compass->set_orientation(compassOrientation);
hal->compass->set_offsets(0,0,0);
hal->compass->set_declination(0.0);
hal->compass->init();
}
if (rangeFinderLeftEnabled) {
hal->debug->println_P(PSTR("initializing left range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(3);
rangeFinder->set_orientation(0, -1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
/**
* 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 NULL 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)
*/
if (rangeFinderRightEnabled) {
hal->debug->println_P(PSTR("initializing right range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(4);
rangeFinder->set_orientation(0, 1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderFrontEnabled) {
hal->debug->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(1);
rangeFinder->set_orientation(1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderUpEnabled) {
hal->debug->println_P(PSTR("initializing up range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(5);
rangeFinder->set_orientation(0, 0, -1);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderBackEnabled) {
hal->debug->println_P(PSTR("initializing back range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(2);
rangeFinder->set_orientation(-1, 0, 0);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderDownEnabled) {
hal->debug->println_P(PSTR("initializing down range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(6);
rangeFinder->set_orientation(0, 0, 1);
hal->rangeFinders.push_back(rangeFinder);
}
if (rangeFinderLeftEnabled) {
hal->debug->println_P(PSTR("initializing left range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(3);
rangeFinder->set_orientation(0, -1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
}
if (rangeFinderRightEnabled) {
hal->debug->println_P(PSTR("initializing right range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(4);
rangeFinder->set_orientation(0, 1, 0);
hal->rangeFinders.push_back(rangeFinder);
}
/*
* Select guidance, navigation, control algorithms
*/
navigator = new NAVIGATOR_CLASS(hal);
guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim);
controller = new CONTROLLER_CLASS(navigator,guide,hal);
if (rangeFinderUpEnabled) {
hal->debug->println_P(PSTR("initializing up range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(5);
rangeFinder->set_orientation(0, 0, -1);
hal->rangeFinders.push_back(rangeFinder);
}
/*
* CommLinks
*/
if (board==BOARD_ARDUPILOTMEGA_2)
{
Serial2.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
}
else
{
Serial3.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
}
if (rangeFinderDownEnabled) {
hal->debug->println_P(PSTR("initializing down range finder"));
RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter);
rangeFinder->set_analog_port(6);
rangeFinder->set_orientation(0, 0, 1);
hal->rangeFinders.push_back(rangeFinder);
}
/*
* Hardware in the Loop
*/
if (hal->getMode() == MODE_HIL_CNTL) {
Serial.println("HIL line setting up");
Serial1.begin(hilBaud, 128, 128);
delay(1000);
Serial1.println("starting hil");
hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
}
}
/*
* Start the autopilot
*/
hal->debug->printf_P(PSTR("initializing autopilot\n"));
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
/*
* Select guidance, navigation, control algorithms
*/
navigator = new NAVIGATOR_CLASS(hal);
guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim);
controller = new CONTROLLER_CLASS(navigator,guide,hal);
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate);
/*
* CommLinks
*/
if (board==BOARD_ARDUPILOTMEGA_2)
{
Serial2.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
}
else
{
Serial3.begin(telemBaud, 128, 128); // gcs
hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
}
/*
* Hardware in the Loop
*/
if (hal->getMode() == MODE_HIL_CNTL) {
Serial.println("HIL line setting up");
Serial1.begin(hilBaud, 128, 128);
delay(1000);
Serial1.println("starting hil");
hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
}
/*
* Start the autopilot
*/
hal->debug->printf_P(PSTR("initializing autopilot\n"));
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate);
}
void loop() {
autoPilot->update();
autoPilot->update();
}
#endif //_APO_COMMON_H

8
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -16,8 +16,8 @@ void AP_ArmingMechanism::update(const float dt) {
// arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
// always start clock at 0
if (_armingClock<0) _armingClock = 0;
@ -31,8 +31,8 @@ void AP_ArmingMechanism::update(const float dt) {
}
// disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
// always start clock at 0
if (_armingClock>0) _armingClock = 0;

8
libraries/APO/AP_ArmingMechanism.h
View File

@ -26,16 +26,16 @@ public:
* @param ch2Max: arms above this
*/
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal,
uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
_ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
_ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
}
/**
* update
*
* arming:
*
*
* to arm: put stick to bottom right for 100 controller cycles
* (max yaw, min throttle)
*

404
libraries/APO/AP_Autopilot.cpp
View File

@ -13,242 +13,242 @@ namespace apo {
class AP_HardwareAbstractionLayer;
AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) :
Loop(loopRate, callback, this), _navigator(navigator), _guide(guide),
_controller(controller), _hal(hal),
callbackCalls(0) {
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) :
Loop(loopRate, callback, this), _navigator(navigator), _guide(guide),
_controller(controller), _hal(hal),
callbackCalls(0) {
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
/*
* Radio setup
*/
hal->debug->println_P(PSTR("initializing radio"));
APM_RC.Init(); // APM Radio initialization,
/*
* Radio setup
*/
hal->debug->println_P(PSTR("initializing radio"));
APM_RC.Init(); // APM Radio initialization,
/*
* Calibration
*/
hal->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
/*
* Calibration
*/
hal->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
if (navigator) navigator->calibrate();
if (navigator) navigator->calibrate();
/*
* Look for valid initial state
*/
while (_navigator) {
// letc gcs known we are alive
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
if (hal->getMode() == MODE_LIVE) {
_navigator->updateSlow(0);
if (hal->gps) {
if (hal->gps->fix) {
break;
} else {
hal->gps->update();
hal->gcs->sendText(SEVERITY_LOW,
PSTR("waiting for gps lock\n"));
hal->debug->printf_P(PSTR("waiting for gps lock\n"));
}
} else { // no gps, can skip
break;
}
} else if (hal->getMode() == MODE_HIL_CNTL) { // hil
hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->hil->receive();
Serial.println("HIL Receive Called");
if (_navigator->getTimeStamp() != 0) {
// give hil a chance to send some packets
for (int i = 0; i < 5; i++) {
hal->debug->println_P(PSTR("reading initial hil packets"));
hal->gcs->sendText(SEVERITY_LOW,
PSTR("reading initial hil packets"));
delay(1000);
}
break;
}
hal->debug->println_P(PSTR("waiting for hil packet"));
}
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();
_hal->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();
_hal->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());
/*
* Look for valid initial state
*/
while (_navigator) {
// letc gcs known we are alive
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
if (hal->getMode() == MODE_LIVE) {
_navigator->updateSlow(0);
if (hal->gps) {
if (hal->gps->fix) {
break;
} else {
hal->gps->update();
hal->gcs->sendText(SEVERITY_LOW,
PSTR("waiting for gps lock\n"));
hal->debug->printf_P(PSTR("waiting for gps lock\n"));
}
} else { // no gps, can skip
break;
}
} else if (hal->getMode() == MODE_HIL_CNTL) { // hil
hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->hil->receive();
Serial.println("HIL Receive Called");
if (_navigator->getTimeStamp() != 0) {
// give hil a chance to send some packets
for (int i = 0; i < 5; i++) {
hal->debug->println_P(PSTR("reading initial hil packets"));
hal->gcs->sendText(SEVERITY_LOW,
PSTR("reading initial hil packets"));
delay(1000);
}
break;
}
hal->debug->println_P(PSTR("waiting for hil packet"));
}
delay(500);
}
/*
* Attach loops, stacking for priority
*/
hal->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));
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();
_hal->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();
_hal->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());
hal->debug->println_P(PSTR("running"));
hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
hal->setState(MAV_STATE_STANDBY);
/*
* Attach loops, stacking for priority
*/
hal->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));
hal->debug->println_P(PSTR("running"));
hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
hal->setState(MAV_STATE_STANDBY);
}
void AP_Autopilot::callback(void * data) {
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->hal()->debug->println_P(PSTR("callback"));
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->hal()->debug->println_P(PSTR("callback"));
/*
* ahrs update
*/
apo->callbackCalls++;
if (apo->getNavigator())
apo->getNavigator()->updateFast(apo->dt());
/*
* 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->getHal()->debug->println_P(PSTR("callback 0"));
/*
* hardware in the loop
*/
if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) {
apo->getHal()->hil->receive();
apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
}
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 0"));
/*
* update control laws
*/
if (apo->getController()) {
//apo->getHal()->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->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg);
*/
* hardware in the loop
*/
if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) {
apo->getHal()->hil->receive();
apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
}
/*
* update control laws
*/
if (apo->getController()) {
//apo->getHal()->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->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg);
*/
}
void AP_Autopilot::callback1(void * data) {
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 1"));
/*
* update guidance laws
*/
if (apo->getGuide())
{
//apo->getHal()->debug->println_P(PSTR("updating guide"));
apo->getGuide()->update();
}
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 1"));
/*
* slow navigation loop update
*/
if (apo->getNavigator()) {
apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt());
}
/*
* update guidance laws
*/
if (apo->getGuide())
{
//apo->getHal()->debug->println_P(PSTR("updating guide"));
apo->getGuide()->update();
}
/*
* send telemetry
*/
if (apo->getHal()->gcs) {
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
}
/*
* slow navigation loop update
*/
if (apo->getNavigator()) {
apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt());
}
/*
* handle ground control station communication
*/
if (apo->getHal()->gcs) {
// send messages
apo->getHal()->gcs->requestCmds();
apo->getHal()->gcs->sendParameters();
/*
* send telemetry
*/
if (apo->getHal()->gcs) {
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
}
// receive messages
apo->getHal()->gcs->receive();
}
/*
* handle ground control station communication
*/
if (apo->getHal()->gcs) {
// send messages
apo->getHal()->gcs->requestCmds();
apo->getHal()->gcs->sendParameters();
/*
* navigator debug
*/
/*
if (apo->navigator()) {
apo->getHal()->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->getHal()->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());
}
*/
// receive messages
apo->getHal()->gcs->receive();
}
/*
* navigator debug
*/
/*
if (apo->navigator()) {
apo->getHal()->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->getHal()->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->getHal()->debug->println_P(PSTR("callback 2"));
/*
* send telemetry
*/
if (apo->getHal()->gcs) {
// send messages
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 2"));
/*
* update battery monitor
*/
if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update();
/*
* send telemetry
*/
if (apo->getHal()->gcs) {
// send messages
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}
/*
* send heartbeat
*/
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
/*
* update battery monitor
*/
if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update();
/*
* load/loop rate/ram debug
*/
apo->getHal()->load = apo->load();
apo->getHal()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls);
apo->callbackCalls = 0;
apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
apo->load(),1.0/apo->dt(),freeMemory());
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
/*
* send heartbeat
*/
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
/*
* 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));
/*
* load/loop rate/ram debug
*/
apo->getHal()->load = apo->load();
apo->getHal()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls);
apo->callbackCalls = 0;
apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
apo->load(),1.0/apo->dt(),freeMemory());
apo->getHal()->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->getHal()->debug->println_P(PSTR("callback 3"));
//AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 3"));
}
} // apo

142
libraries/APO/AP_Autopilot.h
View File

@ -65,88 +65,88 @@ class AP_CommLink;
*/
class AP_Autopilot: public Loop {
public:
/**
* Default constructor
*/
AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate);
/**
* Default constructor
*/
AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
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_HardwareAbstractionLayer * getHal() {
return _hal;
}
/**
* Accessors
*/
AP_Navigator * getNavigator() {
return _navigator;
}
AP_Guide * getGuide() {
return _guide;
}
AP_Controller * getController() {
return _controller;
}
AP_HardwareAbstractionLayer * getHal() {
return _hal;
}
/**
* Loop Monitoring
*/
uint32_t callbackCalls;
/**
* 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 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 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 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 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);
/**
* 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_HardwareAbstractionLayer * _hal;
/**
* Components
*/
AP_Navigator * _navigator;
AP_Guide * _guide;
AP_Controller * _controller;
AP_HardwareAbstractionLayer * _hal;
/**
* Constants
*/
static const float deg2rad = M_PI / 180;
static const float rad2deg = 180 / M_PI;
/**
* Constants
*/
static const float deg2rad = M_PI / 180;
static const float rad2deg = 180 / M_PI;
};
} // namespace apo

1092
libraries/APO/AP_CommLink.cpp
View File

File diff suppressed because it is too large Load Diff

118
libraries/APO/AP_CommLink.h
View File

@ -34,7 +34,7 @@ class AP_Guide;
class AP_HardwareAbstractionLayer;
enum {
SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH
SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH
};
// forward declarations
@ -45,83 +45,83 @@ enum {
class AP_CommLink {
public:
AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
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;
AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
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;
protected:
FastSerial * _link;
AP_Navigator * _navigator;
AP_Guide * _guide;
AP_Controller * _controller;
AP_HardwareAbstractionLayer * _hal;
FastSerial * _link;
AP_Navigator * _navigator;
AP_Guide * _guide;
AP_Controller * _controller;
AP_HardwareAbstractionLayer * _hal;
};
class MavlinkComm: public AP_CommLink {
public:
MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
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);
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();
/**
* sends parameters one at a time
*/
void sendParameters();
/**
* request commands one at a time
*/
void requestCmds();
/**
* request commands one at a time
*/
void requestCmds();
private:
// options
bool _useRelativeAlt;
// 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;
// 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;
// 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;
// channel
mavlink_channel_t _channel;
uint16_t _packetDrops;
static uint8_t _nChannels;
void _handleMessage(mavlink_message_t * msg);
void _handleMessage(mavlink_message_t * msg);
uint16_t _countParameters();
uint16_t _countParameters();
AP_Var * _findParameter(uint16_t index);
AP_Var * _findParameter(uint16_t index);
// check the target
uint8_t _checkTarget(uint8_t sysid, uint8_t compid);
// check the target
uint8_t _checkTarget(uint8_t sysid, uint8_t compid);
};

112
libraries/APO/AP_Controller.cpp
View File

@ -19,79 +19,79 @@
namespace apo {
AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism,
const uint8_t chMode, const uint16_t key, const prog_char_t * name) :
_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
_group(key, name ? : PSTR("CNTRL_")),
_chMode(chMode),
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
setAllRadioChannelsToNeutral();
AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism,
const uint8_t chMode, const uint16_t key, const prog_char_t * name) :
_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
_group(key, name ? : PSTR("CNTRL_")),
_chMode(chMode),
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
setAllRadioChannelsToNeutral();
}
void AP_Controller::setAllRadioChannelsToNeutral() {
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
_hal->rc[i]->setPosition(0.0);
}
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
_hal->rc[i]->setPosition(0.0);
}
}
void AP_Controller::setAllRadioChannelsManually() {
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
_hal->rc[i]->setUsingRadio();
}
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
_hal->rc[i]->setUsingRadio();
}
}
void AP_Controller::update(const float dt) {
if (_armingMechanism) _armingMechanism->update(dt);
if (_armingMechanism) _armingMechanism->update(dt);
// determine flight mode
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
// manual/auto switch
} else {
// if all emergencies cleared, fall back to standby
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
else _mode = MAV_MODE_AUTO;
}
// determine flight mode
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
_mode = MAV_MODE_FAILSAFE;
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
// manual/auto switch
} else {
// if all emergencies cleared, fall back to standby
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
else _mode = MAV_MODE_AUTO;
}
// handle flight modes
switch(_mode) {
// handle flight modes
switch(_mode) {
case MAV_MODE_LOCKED: {
_hal->setState(MAV_STATE_STANDBY);
break;
}
case MAV_MODE_LOCKED: {
_hal->setState(MAV_STATE_STANDBY);
break;
}
case MAV_MODE_FAILSAFE: {
_hal->setState(MAV_STATE_EMERGENCY);
break;
}
case MAV_MODE_FAILSAFE: {
_hal->setState(MAV_STATE_EMERGENCY);
break;
}
case MAV_MODE_MANUAL: {
manualLoop(dt);
break;
}
case MAV_MODE_MANUAL: {
manualLoop(dt);
break;
}
case MAV_MODE_AUTO: {
autoLoop(dt);
break;
}
case MAV_MODE_AUTO: {
autoLoop(dt);
break;
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY);
}
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY);
}
}
// this sends commands to motors
setMotors();
// this sends commands to motors
setMotors();
}
} // namespace apo

390
libraries/APO/AP_Controller.h
View File

@ -38,281 +38,281 @@ class AP_ArmingMechanism;
/// Controller class
class AP_Controller {
public:
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal,
AP_ArmingMechanism * armingMechanism,
const uint8_t _chMode,
const uint16_t key = k_cntrl,
const prog_char_t * name = NULL);
virtual void update(const float dt);
virtual void setMotors() = 0;
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal,
AP_ArmingMechanism * armingMechanism,
const uint8_t _chMode,
const uint16_t key = k_cntrl,
const prog_char_t * name = NULL);
virtual void update(const float dt);
virtual void setMotors() = 0;
void setAllRadioChannelsToNeutral();
void setAllRadioChannelsManually();
virtual void manualLoop(const float dt) {
void setAllRadioChannelsManually();
virtual void manualLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
virtual void autoLoop(const float dt) {
virtual void autoLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
AP_Uint8 getMode() {
return _mode;
}
protected:
AP_Navigator * _nav;
AP_Guide * _guide;
AP_HardwareAbstractionLayer * _hal;
AP_ArmingMechanism * _armingMechanism;
AP_Navigator * _nav;
AP_Guide * _guide;
AP_HardwareAbstractionLayer * _hal;
AP_ArmingMechanism * _armingMechanism;
uint8_t _chMode;
AP_Var_group _group;
AP_Uint8 _mode;
AP_Var_group _group;
AP_Uint8 _mode;
};
class AP_ControllerBlock {
public:
AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength) :
_group(group), _groupStart(groupStart),
_groupEnd(groupStart + groupLength) {
}
uint8_t getGroupEnd() {
return _groupEnd;
}
AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength) :
_group(group), _groupStart(groupStart),
_groupEnd(groupStart + groupLength) {
}
uint8_t getGroupEnd() {
return _groupEnd;
}
protected:
AP_Var_group * _group; /// helps with parameter management
uint8_t _groupStart;
uint8_t _groupEnd;
AP_Var_group * _group; /// helps with parameter management
uint8_t _groupStart;
uint8_t _groupEnd;
};
class BlockLowPass: public AP_ControllerBlock {
public:
BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
_y(0) {
}
float 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;
}
BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
_y(0) {
}
float 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;
}
protected:
AP_Float _fCut;
float _y;
AP_Float _fCut;
float _y;
};
class BlockSaturation: public AP_ControllerBlock {
public:
BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
}
float update(const float & input) {
BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
}
float update(const float & input) {
// pid sum
float y = input;
// pid sum
float y = input;
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
protected:
AP_Float _yMax; /// output saturation
AP_Float _yMax; /// output saturation
};
class BlockDerivative {
public:
BlockDerivative() :
_lastInput(0), firstRun(true) {
}
float update(const float & input, const float & dt) {
float derivative = (input - _lastInput) / dt;
_lastInput = input;
if (firstRun) {
firstRun = false;
return 0;
} else
return derivative;
}
BlockDerivative() :
_lastInput(0), firstRun(true) {
}
float update(const float & input, const float & dt) {
float derivative = (input - _lastInput) / dt;
_lastInput = input;
if (firstRun) {
firstRun = false;
return 0;
} else
return derivative;
}
protected:
float _lastInput; /// last input
bool firstRun;
float _lastInput; /// last input
bool firstRun;
};
class BlockIntegral {
public:
BlockIntegral() :
_i(0) {
}
float update(const float & input, const float & dt) {
_i += input * dt;
return _i;
}
BlockIntegral() :
_i(0) {
}
float update(const float & input, const float & dt) {
_i += input * dt;
return _i;
}
protected:
float _i; /// integral
float _i; /// integral
};
class BlockP: public AP_ControllerBlock {
public:
BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
}
BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
}
float update(const float & input) {
return _kP * input;
}
float update(const float & input) {
return _kP * input;
}
protected:
AP_Float _kP; /// proportional gain
AP_Float _kP; /// proportional 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) :
AP_ControllerBlock(group, groupStart, 2),
_kI(group, groupStart, kI, kILabel ? : PSTR("i")),
_blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
_eI(0) {
}
BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
const prog_char_t * kILabel = NULL,
const prog_char_t * iMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 2),
_kI(group, groupStart, kI, kILabel ? : PSTR("i")),
_blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
_eI(0) {
}
float update(const float & input, const float & dt) {
// integral
_eI += input * dt;
_eI = _blockSaturation.update(_eI);
return _kI * _eI;
}
float update(const float & input, const float & dt) {
// integral
_eI += input * dt;
_eI = _blockSaturation.update(_eI);
return _kI * _eI;
}
protected:
AP_Float _kI; /// integral gain
BlockSaturation _blockSaturation; /// integrator saturation
float _eI; /// integral of input
AP_Float _kI; /// integral gain
BlockSaturation _blockSaturation; /// integrator saturation
float _eI; /// integral of input
};
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) :
AP_ControllerBlock(group, groupStart, 2),
_blockLowPass(group, groupStart, dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD,
kDLabel ? : PSTR("d")), _eP(0) {
}
float update(const float & input, const float & dt) {
// derivative with low pass
float _eD = _blockLowPass.update((_eP - input) / dt, dt);
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
return _kD * _eD;
}
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) :
AP_ControllerBlock(group, groupStart, 2),
_blockLowPass(group, groupStart, dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD,
kDLabel ? : PSTR("d")), _eP(0) {
}
float update(const float & input, const float & dt) {
// derivative with low pass
float _eD = _blockLowPass.update((_eP - input) / dt, dt);
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
return _kD * _eD;
}
protected:
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
float _eP; /// input
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
float _eP; /// input
};
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) :
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) {
}
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 update(const float & input, const float & dt) {
return _blockSaturation.update(
_blockP.update(input) + _blockI.update(input, dt)
+ _blockD.update(input, dt));
}
float update(const float & input, const float & dt) {
return _blockSaturation.update(
_blockP.update(input) + _blockI.update(input, dt)
+ _blockD.update(input, dt));
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockD _blockD;
BlockSaturation _blockSaturation;
BlockP _blockP;
BlockI _blockI;
BlockD _blockD;
BlockSaturation _blockSaturation;
};
class BlockPI: public AP_ControllerBlock {
public:
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) {
}
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 update(const float & input, const float & dt) {
float update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt);
return _blockSaturation.update(y);
}
float y = _blockP.update(input) + _blockI.update(input, dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
};
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) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockD(group, _blockP.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
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 update(const float & input, const float & dt) {
float update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockD.update(input, dt);
return _blockSaturation.update(y);
}
float y = _blockP.update(input) + _blockD.update(input, dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockD _blockD;
BlockSaturation _blockSaturation;
BlockP _blockP;
BlockD _blockD;
BlockSaturation _blockSaturation;
};
/// 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, float dFCut,
const prog_char_t * dFCutLabel = NULL,
const prog_char_t * dLabel = NULL) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
return _blockSaturation.update(y);
}
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel = NULL,
const prog_char_t * dLabel = NULL) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
};
} // apo

298
libraries/APO/AP_Guide.cpp
View File

@ -15,65 +15,65 @@
namespace apo {
AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) :
_navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
_previousCommand(AP_MavlinkCommand::home),
_headingCommand(0), _airSpeedCommand(0),
_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
_pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
_numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
_nextCommandTimer(0) {
_navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
_previousCommand(AP_MavlinkCommand::home),
_headingCommand(0), _airSpeedCommand(0),
_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
_pECmd(0), _pDCmd(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());
_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT);
void AP_Guide::setCurrentIndex(uint8_t val) {
_cmdIndex.set_and_save(val);
_command = AP_MavlinkCommand(getCurrentIndex());
_previousCommand = AP_MavlinkCommand(getPreviousIndex());
_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT);
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
_rangeFinderLeft(), _rangeFinderRight(),
_group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
_rangeFinderLeft(), _rangeFinderRight(),
_group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
else if (rF->orientation_x == -1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderBack = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == 1
&& rF->orientation_z == 0)
_rangeFinderRight = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == -1
&& rF->orientation_z == 0)
_rangeFinderLeft = rF;
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
else if (rF->orientation_x == -1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderBack = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == 1
&& rF->orientation_z == 0)
_rangeFinderRight = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == -1
&& rF->orientation_z == 0)
_rangeFinderLeft = rF;
}
}
}
void MavlinkGuide::update() {
// process mavlink commands
handleCommand();
// process mavlink commands
handleCommand();
// obstacle avoidance overrides
// stop if your going to drive into something in front of you
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++)
_hal->rangeFinders[i]->read();
float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
if (_rangeFinderFront && frontDistance < 2) {
_mode = MAV_NAV_VECTOR;
// obstacle avoidance overrides
// stop if your going to drive into something in front of you
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++)
_hal->rangeFinders[i]->read();
float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
if (_rangeFinderFront && frontDistance < 2) {
_mode = MAV_NAV_VECTOR;
//airSpeedCommand = 0;
//groundSpeedCommand = 0;
//airSpeedCommand = 0;
//groundSpeedCommand = 0;
// _headingCommand -= 45 * deg2Rad;
// _hal->debug->print("Obstacle Distance (m): ");
// _hal->debug->println(frontDistance);
@ -82,120 +82,120 @@ void MavlinkGuide::update() {
// _hal->debug->printf_P(
// PSTR("Front Distance, %f\n"),
// frontDistance);
}
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
}
if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
_airSpeedCommand = 0;
_groundSpeedCommand = 0;
}
}
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;
}
// 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;
}
_cmdIndex = getNextIndex();
//Serial.print("cmd : "); Serial.println(int(_cmdIndex));
//Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex()));
//Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands()));
_command = AP_MavlinkCommand(getCurrentIndex());
_previousCommand = AP_MavlinkCommand(getPreviousIndex());
_cmdIndex = getNextIndex();
//Serial.print("cmd : "); Serial.println(int(_cmdIndex));
//Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex()));
//Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands()));
_command = AP_MavlinkCommand(getCurrentIndex());
_previousCommand = AP_MavlinkCommand(getPreviousIndex());
}
void MavlinkGuide::handleCommand() {
// TODO handle more commands
switch (_command.getCommand()) {
// TODO handle more commands
switch (_command.getCommand()) {
case MAV_CMD_NAV_WAYPOINT: {
case MAV_CMD_NAV_WAYPOINT: {
// if we don't have enough waypoint for cross track calcs
// go home
if (_numberOfCommands == 1) {
_mode = MAV_NAV_RETURNING;
_altitudeCommand = AP_MavlinkCommand::home.getAlt();
_headingCommand = AP_MavlinkCommand::home.bearingTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt())
+ 180 * deg2Rad;
if (_headingCommand > 360 * deg2Rad)
_headingCommand -= 360 * deg2Rad;
// if we don't have enough waypoint for cross track calcs
// go home
if (_numberOfCommands == 1) {
_mode = MAV_NAV_RETURNING;
_altitudeCommand = AP_MavlinkCommand::home.getAlt();
_headingCommand = AP_MavlinkCommand::home.bearingTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt())
+ 180 * deg2Rad;
if (_headingCommand > 360 * deg2Rad)
_headingCommand -= 360 * deg2Rad;
//_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"),
//headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
// if we have 2 or more waypoints do x track navigation
} else {
_mode = MAV_NAV_WAYPOINT;
float alongTrack = _command.alongTrack(_previousCommand,
_navigator->getLat_degInt(),
_navigator->getLon_degInt());
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
float segmentLength = _previousCommand.distanceTo(_command);
if (distanceToNext < _command.getRadius() || alongTrack
> segmentLength)
{
Serial.println("waypoint reached");
nextCommand();
}
_altitudeCommand = _command.getAlt();
float dXt = _command.crossTrack(_previousCommand,
_navigator->getLat_degInt(),
_navigator->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;
//_hal->debug->printf_P(
//PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
//bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
}
//_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"),
//headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()));
_groundSpeedCommand = _velocityCommand;
// if we have 2 or more waypoints do x track navigation
} else {
_mode = MAV_NAV_WAYPOINT;
float alongTrack = _command.alongTrack(_previousCommand,
_navigator->getLat_degInt(),
_navigator->getLon_degInt());
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
float segmentLength = _previousCommand.distanceTo(_command);
if (distanceToNext < _command.getRadius() || alongTrack
> segmentLength)
{
Serial.println("waypoint reached");
nextCommand();
}
_altitudeCommand = _command.getAlt();
float dXt = _command.crossTrack(_previousCommand,
_navigator->getLat_degInt(),
_navigator->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;
//_hal->debug->printf_P(
//PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
//bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
}
// calculate pN,pE,pD from home and gps coordinates
_pNCmd = _command.getPN(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pECmd = _command.getPE(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pDCmd = _command.getPD(_navigator->getAlt_intM());
_groundSpeedCommand = _velocityCommand;
// debug
//_hal->debug->printf_P(
//PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
//getNumberOfCommands(),
//getCurrentIndex(),
//getPreviousIndex());
// calculate pN,pE,pD from home and gps coordinates
_pNCmd = _command.getPN(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pECmd = _command.getPE(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pDCmd = _command.getPD(_navigator->getAlt_intM());
break;
}
// debug
//_hal->debug->printf_P(
//PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
//getNumberOfCommands(),
//getCurrentIndex(),
//getPreviousIndex());
break;
}
// case MAV_CMD_CONDITION_CHANGE_ALT:
// case MAV_CMD_CONDITION_DELAY:
// case MAV_CMD_CONDITION_DISTANCE:
@ -224,12 +224,12 @@ void MavlinkGuide::handleCommand() {
// case MAV_CMD_NAV_PATHPLANNING:
// case MAV_CMD_NAV_RETURN_TO_LAUNCH:
// case MAV_CMD_NAV_TAKEOFF:
default:
// unhandled command, skip
Serial.println("unhandled command");
nextCommand();
break;
}
default:
// unhandled command, skip
Serial.println("unhandled command");
nextCommand();
break;
}
}
} // namespace apo

178
libraries/APO/AP_Guide.h
View File

@ -33,112 +33,112 @@ class AP_HardwareAbstractionLayer;
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 * navigator, AP_HardwareAbstractionLayer * hal);
/**
* This is the constructor, which requires a link to the navigator.
* @param navigator This is the navigator pointer.
*/
AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal);
virtual void update() = 0;
virtual void update() = 0;
virtual void nextCommand() = 0;
virtual void nextCommand() = 0;
MAV_NAV getMode() const {
return _mode;
}
uint8_t getCurrentIndex() {
return _cmdIndex;
}
MAV_NAV getMode() const {
return _mode;
}
uint8_t getCurrentIndex() {
return _cmdIndex;
}
void setCurrentIndex(uint8_t val);
void setCurrentIndex(uint8_t val);
uint8_t getNumberOfCommands() {
return _numberOfCommands;
}
uint8_t getNumberOfCommands() {
return _numberOfCommands;
}
void setNumberOfCommands(uint8_t val) {
_numberOfCommands.set_and_save(val);
}
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 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;
}
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 getHeadingCommand() {
return _headingCommand;
}
float getAirSpeedCommand() {
return _airSpeedCommand;
}
float getGroundSpeedCommand() {
return _groundSpeedCommand;
}
float getAltitudeCommand() {
return _altitudeCommand;
}
float getPNCmd() {
return _pNCmd;
}
float getPECmd() {
return _pECmd;
}
float getPDCmd() {
return _pDCmd;
}
MAV_NAV getMode() {
return _mode;
}
uint8_t getCommandIndex() {
return _cmdIndex;
}
float getHeadingCommand() {
return _headingCommand;
}
float getAirSpeedCommand() {
return _airSpeedCommand;
}
float getGroundSpeedCommand() {
return _groundSpeedCommand;
}
float getAltitudeCommand() {
return _altitudeCommand;
}
float getPNCmd() {
return _pNCmd;
}
float getPECmd() {
return _pECmd;
}
float getPDCmd() {
return _pDCmd;
}
MAV_NAV getMode() {
return _mode;
}
uint8_t getCommandIndex() {
return _cmdIndex;
}
protected:
AP_Navigator * _navigator;
AP_HardwareAbstractionLayer * _hal;
AP_MavlinkCommand _command, _previousCommand;
float _headingCommand;
float _airSpeedCommand;
float _groundSpeedCommand;
float _altitudeCommand;
float _pNCmd;
float _pECmd;
float _pDCmd;
MAV_NAV _mode;
AP_Uint8 _numberOfCommands;
AP_Uint8 _cmdIndex;
uint16_t _nextCommandCalls;
uint16_t _nextCommandTimer;
AP_Navigator * _navigator;
AP_HardwareAbstractionLayer * _hal;
AP_MavlinkCommand _command, _previousCommand;
float _headingCommand;
float _airSpeedCommand;
float _groundSpeedCommand;
float _altitudeCommand;
float _pNCmd;
float _pECmd;
float _pDCmd;
MAV_NAV _mode;
AP_Uint8 _numberOfCommands;
AP_Uint8 _cmdIndex;
uint16_t _nextCommandCalls;
uint16_t _nextCommandTimer;
};
class MavlinkGuide: public AP_Guide {
public:
MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim);
virtual void update();
void nextCommand();
void handleCommand();
MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim);
virtual void update();
void nextCommand();
void handleCommand();
private:
RangeFinder * _rangeFinderFront;
RangeFinder * _rangeFinderBack;
RangeFinder * _rangeFinderLeft;
RangeFinder * _rangeFinderRight;
AP_Var_group _group;
AP_Float _velocityCommand;
AP_Float _crossTrackGain;
AP_Float _crossTrackLim;
RangeFinder * _rangeFinderFront;
RangeFinder * _rangeFinderBack;
RangeFinder * _rangeFinderLeft;
RangeFinder * _rangeFinderRight;
AP_Var_group _group;
AP_Float _velocityCommand;
AP_Float _crossTrackGain;
AP_Float _crossTrackLim;
};
} // namespace apo

239
libraries/APO/AP_HardwareAbstractionLayer.h
View File

@ -28,145 +28,146 @@ class AP_BatteryMonitor;
// enumerations
enum halMode_t {
MODE_LIVE, MODE_HIL_CNTL,
/*MODE_HIL_NAV*/};
MODE_LIVE, MODE_HIL_CNTL,
/*MODE_HIL_NAV*/
};
enum board_t {
BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2
BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2
};
enum vehicle_t {
VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK
VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK
};
class AP_HardwareAbstractionLayer {
public:
// default ctors on pointers called on pointers here, this
// allows NULL to be used as a boolean for if the device was
// initialized
AP_HardwareAbstractionLayer(halMode_t mode, board_t board,
vehicle_t vehicle, uint8_t heartBeatTimeout) :
adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), rc(), gcs(),
hil(), debug(), load(), lastHeartBeat(),
_heartBeatTimeout(heartBeatTimeout), _mode(mode),
_board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) {
// default ctors on pointers called on pointers here, this
// allows NULL to be used as a boolean for if the device was
// initialized
AP_HardwareAbstractionLayer(halMode_t mode, board_t board,
vehicle_t vehicle, uint8_t heartBeatTimeout) :
adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), rc(), gcs(),
hil(), debug(), load(), lastHeartBeat(),
_heartBeatTimeout(heartBeatTimeout), _mode(mode),
_board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) {
/*
* Board specific hardware initialization
*/
if (board == BOARD_ARDUPILOTMEGA_1280) {
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
} else if (board == BOARD_ARDUPILOTMEGA_2560) {
slideSwitchPin = 40;
pushButtonPin = 41;
aLedPin = 37;
bLedPin = 36;
cLedPin = 35;
eepromMaxAddr = 2048;
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
} else if (board == BOARD_ARDUPILOTMEGA_2) {
slideSwitchPin = 40;
pushButtonPin = 41;
aLedPin = 37;
bLedPin = 36;
cLedPin = 35;
eepromMaxAddr = 2048;
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
}
}
/*
* Board specific hardware initialization
*/
if (board == BOARD_ARDUPILOTMEGA_1280) {
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
} else if (board == BOARD_ARDUPILOTMEGA_2560) {
slideSwitchPin = 40;
pushButtonPin = 41;
aLedPin = 37;
bLedPin = 36;
cLedPin = 35;
eepromMaxAddr = 2048;
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
} else if (board == BOARD_ARDUPILOTMEGA_2) {
slideSwitchPin = 40;
pushButtonPin = 41;
aLedPin = 37;
bLedPin = 36;
cLedPin = 35;
eepromMaxAddr = 2048;
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
}
}
/**
* Sensors
*/
AP_ADC * adc;
GPS * gps;
APM_BMP085_Class * baro;
Compass * compass;
Vector<RangeFinder *> rangeFinders;
IMU * imu;
AP_BatteryMonitor * batteryMonitor;
/**
* Sensors
*/
AP_ADC * adc;
GPS * gps;
APM_BMP085_Class * baro;
Compass * compass;
Vector<RangeFinder *> rangeFinders;
IMU * imu;
AP_BatteryMonitor * batteryMonitor;
/**
* Radio Channels
*/
Vector<AP_RcChannel *> rc;
/**
* Radio Channels
*/
Vector<AP_RcChannel *> rc;
/**
* Communication Channels
*/
AP_CommLink * gcs;
AP_CommLink * hil;
FastSerial * debug;
/**
* Communication Channels
*/
AP_CommLink * gcs;
AP_CommLink * hil;
FastSerial * debug;
/**
* data
*/
uint8_t load;
uint32_t lastHeartBeat;
/**
* data
*/
uint8_t load;
uint32_t lastHeartBeat;
/**
* settings
*/
uint8_t slideSwitchPin;
uint8_t pushButtonPin;
uint8_t aLedPin;
uint8_t bLedPin;
uint8_t cLedPin;
uint16_t eepromMaxAddr;
/**
* settings
*/
uint8_t slideSwitchPin;
uint8_t pushButtonPin;
uint8_t aLedPin;
uint8_t bLedPin;
uint8_t cLedPin;
uint16_t eepromMaxAddr;
// accessors
halMode_t getMode() {
return _mode;
}
board_t getBoard() {
return _board;
}
vehicle_t getVehicle() {
return _vehicle;
}
MAV_STATE getState() {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
// accessors
halMode_t getMode() {
return _mode;
}
board_t getBoard() {
return _board;
}
vehicle_t getVehicle() {
return _vehicle;
}
MAV_STATE getState() {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
bool heartBeatLost() {
if (_heartBeatTimeout == 0)
return false;
else
return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout;
}
bool heartBeatLost() {
if (_heartBeatTimeout == 0)
return false;
else
return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout;
}
private:
// enumerations
uint8_t _heartBeatTimeout;
halMode_t _mode;
board_t _board;
vehicle_t _vehicle;
MAV_STATE _state;
// enumerations
uint8_t _heartBeatTimeout;
halMode_t _mode;
board_t _board;
vehicle_t _vehicle;
MAV_STATE _state;
};
}

286
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -10,175 +10,195 @@
namespace apo {
AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) :
_data(v._data), _seq(v._seq) {
//if (FastSerial::getInitialized(0)) Serial.println("copy ctor");
_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) {
_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("++");
}
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;
// 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;
}
// 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();
_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
if (cmd.seq == 0) home.load();
// reload home if sent
if (cmd.seq == 0) 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);
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();
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();
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;
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 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;
// 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 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;
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;
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 {
// ignores lat/lon since single prec.
float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
float d = previous.distanceTo(lat_degInt, lon_degInt);
return dXt / tan(asin(dXt / d));
int32_t lat_degInt, int32_t lon_degInt) const {
// ignores lat/lon since single prec.
float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
float d = previous.distanceTo(lat_degInt, lon_degInt);
return dXt / tan(asin(dXt / d));
}

652
libraries/APO/AP_MavlinkCommand.h
View File

@ -18,355 +18,355 @@ 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;
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;
static AP_MavlinkCommand home;
/**
* Copy Constructor
*/
AP_MavlinkCommand(const AP_MavlinkCommand & v);
/**
* Copy Constructor
*/
AP_MavlinkCommand(const AP_MavlinkCommand & v);
/**
* Basic Constructor
* @param index Start at zero.
*/
AP_MavlinkCommand(uint16_t index, bool doLoad = true);
/**
* 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);
/**
* 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 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;
}
}
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 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();
}
float getRadius() const {
return getParam2();
}
void setRadius(float val) {
setParam2(val);
}
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;
/**
* 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;
/**
* 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;
/**
* 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 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;
/**
* 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 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 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 getPD(int32_t alt_intM) const {
return -(alt_intM / scale_m - getAlt());
}
float getLat(float pN) const {
float getLat(float pN) const {
return pN / rEarth + getLat();
}
return pN / rEarth + getLat();
}
float getLon(float pE) const {
float getLon(float pE) const {
return pE / rEarth / cos(getLat()) + getLon();
}
return pE / rEarth / cos(getLat()) + getLon();
}
/**
* Gets altitude in meters
* @param pD alt in meters
* @return
*/
float getAlt(float pD) const {
/**
* Gets altitude in meters
* @param pD alt in meters
* @return
*/
float getAlt(float pD) const {
return getAlt() - pD;
}
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 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;
// 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

256
libraries/APO/AP_Navigator.cpp
View File

@ -20,187 +20,187 @@
namespace apo {
AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
_hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
_pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
_groundSpeed(0), _vD(0), _lat_degInt(0),
_lon_degInt(0), _alt_intM(0) {
_hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
_pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
_groundSpeed(0), _vD(0), _lat_degInt(0),
_lon_degInt(0), _alt_intM(0) {
}
void AP_Navigator::calibrate() {
}
float AP_Navigator::getPD() const {
return AP_MavlinkCommand::home.getPD(getAlt_intM());
return AP_MavlinkCommand::home.getPD(getAlt_intM());
}
float AP_Navigator::getPE() const {
return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
}
float AP_Navigator::getPN() const {
return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
}
void AP_Navigator::setPD(float _pD) {
setAlt(AP_MavlinkCommand::home.getAlt(_pD));
setAlt(AP_MavlinkCommand::home.getAlt(_pD));
}
void AP_Navigator::setPE(float _pE) {
setLat(AP_MavlinkCommand::home.getLat(_pE));
setLat(AP_MavlinkCommand::home.getLat(_pE));
}
void AP_Navigator::setPN(float _pN) {
setLon(AP_MavlinkCommand::home.getLon(_pN));
setLon(AP_MavlinkCommand::home.getLon(_pN));
}
DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) :
AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) {
AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) {
// 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 < _hal-> rangeFinders.getSize(); i++) {
if (_hal->rangeFinders[i] == NULL)
continue;
if (_hal->rangeFinders[i]->orientation_x == 0
&& _hal->rangeFinders[i]->orientation_y == 0
&& _hal->rangeFinders[i]->orientation_z == 1)
_rangeFinderDown = _hal->rangeFinders[i];
}
// 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 < _hal-> rangeFinders.getSize(); i++) {
if (_hal->rangeFinders[i] == NULL)
continue;
if (_hal->rangeFinders[i]->orientation_x == 0
&& _hal->rangeFinders[i]->orientation_y == 0
&& _hal->rangeFinders[i]->orientation_z == 1)
_rangeFinderDown = _hal->rangeFinders[i];
}
if (_hal->getMode() == MODE_LIVE) {
if (_hal->getMode() == MODE_LIVE) {
if (_hal->adc) {
_hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib);
}
if (_hal->adc) {
_hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib);
}
if (_hal->imu) {
_dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass);
if (_hal->imu) {
_dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass);
// tune down dcm
_dcm->kp_roll_pitch(0.030000);
_dcm->ki_roll_pitch(0.00001278), // 50 hz I term
// 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);
}
// tune down compass in dcm
_dcm->kp_yaw(0.08);
_dcm->ki_yaw(0);
}
if (_hal->compass) {
_dcm->set_compass(_hal->compass);
}
}
if (_hal->compass) {
_dcm->set_compass(_hal->compass);
}
}
}
void DcmNavigator::calibrate() {
AP_Navigator::calibrate();
AP_Navigator::calibrate();
// TODO: handle cold/warm restart
if (_hal->imu) {
_hal->imu->init(IMU::COLD_START,delay);
}
// TODO: handle cold/warm restart
if (_hal->imu) {
_hal->imu->init(IMU::COLD_START,delay);
}
}
void DcmNavigator::updateFast(float dt) {
if (_hal->getMode() != MODE_LIVE)
return;
if (_hal->getMode() != MODE_LIVE)
return;
setTimeStamp(micros()); // if running in live mode, record new time stamp
setTimeStamp(micros()); // if running in live mode, record new time stamp
//_hal->debug->println_P(PSTR("nav loop"));
//_hal->debug->println_P(PSTR("nav loop"));
/**
* 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.
*/
if (_hal->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.
*/
if (_hal->baro) {
if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695)
setAlt(_rangeFinderDown->distance);
if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695)
setAlt(_rangeFinderDown->distance);
else {
float tmp = (_hal->baro->Press / 101325.0);
tmp = pow(tmp, 0.190295);
//setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press
setAlt(0.0);
}
}
else {
float tmp = (_hal->baro->Press / 101325.0);
tmp = pow(tmp, 0.190295);
//setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press
setAlt(0.0);
}
}
// dcm class for attitude
if (_dcm) {
_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);
// dcm class for attitude
if (_dcm) {
_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);
/*
* accel/gyro debug
*/
/*
Vector3f accel = _hal->imu->get_accel();
Vector3f gyro = _hal->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);
*/
}
/*
* accel/gyro debug
*/
/*
Vector3f accel = _hal->imu->get_accel();
Vector3f gyro = _hal->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 DcmNavigator::updateSlow(float dt) {
if (_hal->getMode() != MODE_LIVE)
return;
if (_hal->getMode() != MODE_LIVE)
return;
setTimeStamp(micros()); // if running in live mode, record new time stamp
setTimeStamp(micros()); // if running in live mode, record new time stamp
if (_hal->gps) {
_hal->gps->update();
updateGpsLight();
if (_hal->gps->fix && _hal->gps->new_data) {
setLat_degInt(_hal->gps->latitude);
setLon_degInt(_hal->gps->longitude);
setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm
setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s
}
}
if (_hal->gps) {
_hal->gps->update();
updateGpsLight();
if (_hal->gps->fix && _hal->gps->new_data) {
setLat_degInt(_hal->gps->latitude);
setLon_degInt(_hal->gps->longitude);
setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm
setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s
}
}
if (_hal->compass) {
_hal->compass->read();
_hal->compass->calculate(_dcm->get_dcm_matrix());
_hal->compass->null_offsets(_dcm->get_dcm_matrix());
//_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading);
}
if (_hal->compass) {
_hal->compass->read();
_hal->compass->calculate(_dcm->get_dcm_matrix());
_hal->compass->null_offsets(_dcm->get_dcm_matrix());
//_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading);
}
}
void DcmNavigator::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 (_hal->gps->status()) {
case (2):
//digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break;
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
static bool GPS_light = false;
switch (_hal->gps->status()) {
case (2):
//digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break;
case (1):
if (_hal->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(_hal->cLedPin, LOW);
} else {
digitalWrite(_hal->cLedPin, HIGH);
}
_hal->gps->valid_read = false;
}
break;
case (1):
if (_hal->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(_hal->cLedPin, LOW);
} else {
digitalWrite(_hal->cLedPin, HIGH);
}
_hal->gps->valid_read = false;
}
break;
default:
digitalWrite(_hal->cLedPin, LOW);
break;
}
default:
digitalWrite(_hal->cLedPin, LOW);
break;
}
}
} // namespace apo

292
libraries/APO/AP_Navigator.h
View File

@ -33,195 +33,195 @@ class AP_HardwareAbstractionLayer;
/// Navigator class
class AP_Navigator {
public:
AP_Navigator(AP_HardwareAbstractionLayer * hal);
virtual void calibrate();
virtual void updateFast(float dt) = 0;
virtual void updateSlow(float dt) = 0;
float getPD() const;
float getPE() const;
float getPN() const;
void setPD(float _pD);
void setPE(float _pE);
void setPN(float _pN);
AP_Navigator(AP_HardwareAbstractionLayer * hal);
virtual void calibrate();
virtual void updateFast(float dt) = 0;
virtual void updateSlow(float dt) = 0;
float getPD() const;
float getPE() const;
float getPN() const;
void setPD(float _pD);
void setPE(float _pE);
void setPN(float _pN);
float getAirSpeed() const {
return _airSpeed;
}
float getAirSpeed() const {
return _airSpeed;
}
int32_t getAlt_intM() const {
return _alt_intM;
}
int32_t getAlt_intM() const {
return _alt_intM;
}
float getAlt() const {
return _alt_intM / scale_m;
}
float getAlt() const {
return _alt_intM / scale_m;
}
void setAlt(float _alt) {
this->_alt_intM = _alt * 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;
}
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);
}
void setLat(float _lat) {
//Serial.print("setLatfirst");
//Serial.println(_lat * rad2DegInt);
setLat_degInt(_lat*rad2DegInt);
}
float getLon() const {
return _lon_degInt * degInt2Rad;
}
float getLon() const {
return _lon_degInt * degInt2Rad;
}
void setLon(float _lon) {
this->_lon_degInt = _lon * rad2DegInt;
}
void setLon(float _lon) {
this->_lon_degInt = _lon * rad2DegInt;
}
float getVD() const {
return _vD;
}
float getVD() const {
return _vD;
}
float getVE() const {
return sin(getYaw()) * getGroundSpeed();
}
float getVE() const {
return sin(getYaw()) * getGroundSpeed();
}
float getGroundSpeed() const {
return _groundSpeed;
}
float getGroundSpeed() const {
return _groundSpeed;
}
int32_t getLat_degInt() const {
//Serial.print("getLat_degInt");
//Serial.println(_lat_degInt);
return _lat_degInt;
int32_t getLat_degInt() const {
//Serial.print("getLat_degInt");
//Serial.println(_lat_degInt);
return _lat_degInt;
}
}
int32_t getLon_degInt() const {
return _lon_degInt;
}
int32_t getLon_degInt() const {
return _lon_degInt;
}
float getVN() const {
return cos(getYaw()) * getGroundSpeed();
}
float getVN() const {
return cos(getYaw()) * getGroundSpeed();
}
float getPitch() const {
return _pitch;
}
float getPitch() const {
return _pitch;
}
float getPitchRate() const {
return _pitchRate;
}
float getPitchRate() const {
return _pitchRate;
}
float getRoll() const {
return _roll;
}
float getRoll() const {
return _roll;
}
float getRollRate() const {
return _rollRate;
}
float getRollRate() const {
return _rollRate;
}
float getYaw() const {
return _yaw;
}
float getYaw() const {
return _yaw;
}
float getYawRate() const {
return _yawRate;
}
float getYawRate() const {
return _yawRate;
}
void setAirSpeed(float airSpeed) {
_airSpeed = airSpeed;
}
void setAirSpeed(float airSpeed) {
_airSpeed = airSpeed;
}
void setAlt_intM(int32_t alt_intM) {
_alt_intM = alt_intM;
}
void setAlt_intM(int32_t alt_intM) {
_alt_intM = alt_intM;
}
void setVD(float vD) {
_vD = vD;
}
void setVD(float vD) {
_vD = vD;
}
void setGroundSpeed(float groundSpeed) {
_groundSpeed = groundSpeed;
}
void setGroundSpeed(float groundSpeed) {
_groundSpeed = groundSpeed;
}
void setLat_degInt(int32_t lat_degInt) {
_lat_degInt = lat_degInt;
//Serial.print("setLat_degInt");
//Serial.println(_lat_degInt);
}
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 setLon_degInt(int32_t lon_degInt) {
_lon_degInt = lon_degInt;
}
void setPitch(float pitch) {
_pitch = pitch;
}
void setPitch(float pitch) {
_pitch = pitch;
}
void setPitchRate(float pitchRate) {
_pitchRate = pitchRate;
}
void setPitchRate(float pitchRate) {
_pitchRate = pitchRate;
}
void setRoll(float roll) {
_roll = roll;
}
void setRoll(float roll) {
_roll = roll;
}
void setRollRate(float rollRate) {
_rollRate = rollRate;
}
void setRollRate(float rollRate) {
_rollRate = rollRate;
}
void setYaw(float yaw) {
_yaw = yaw;
}
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 setYawRate(float yawRate) {
_yawRate = yawRate;
}
void setTimeStamp(int32_t timeStamp) {
_timeStamp = timeStamp;
}
int32_t getTimeStamp() const {
return _timeStamp;
}
protected:
AP_HardwareAbstractionLayer * _hal;
AP_HardwareAbstractionLayer * _hal;
private:
int32_t _timeStamp; // micros clock
float _roll; // rad
float _rollRate; //rad/s
float _pitch; // rad
float _pitchRate; // rad/s
float _yaw; // rad
float _yawRate; // rad/s
float _airSpeed; // m/s
float _groundSpeed; // m/s
float _vD; // m/s
int32_t _lat_degInt; // deg / 1e7
int32_t _lon_degInt; // deg / 1e7
int32_t _alt_intM; // meters / 1e3
int32_t _timeStamp; // micros clock
float _roll; // rad
float _rollRate; //rad/s
float _pitch; // rad
float _pitchRate; // rad/s
float _yaw; // rad
float _yawRate; // rad/s
float _airSpeed; // m/s
float _groundSpeed; // m/s
float _vD; // m/s
int32_t _lat_degInt; // deg / 1e7
int32_t _lon_degInt; // deg / 1e7
int32_t _alt_intM; // meters / 1e3
};
class DcmNavigator: public AP_Navigator {
private:
/**
* Sensors
*/
/**
* Sensors
*/
RangeFinder * _rangeFinderDown;
AP_DCM * _dcm;
IMU * _imu;
uint16_t _imuOffsetAddress;
RangeFinder * _rangeFinderDown;
AP_DCM * _dcm;
IMU * _imu;
uint16_t _imuOffsetAddress;
public:
DcmNavigator(AP_HardwareAbstractionLayer * hal);
virtual void calibrate();
virtual void updateFast(float dt);
virtual void updateSlow(float dt);
void updateGpsLight(void);
DcmNavigator(AP_HardwareAbstractionLayer * hal);
virtual void calibrate();
virtual void updateFast(float dt);
virtual void updateSlow(float dt);
void updateGpsLight(void);
};
} // namespace apo

124
libraries/APO/AP_RcChannel.cpp
View File

@ -18,85 +18,85 @@
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);
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);
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");
//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 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;
// 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);
//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;
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;
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;
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

96
libraries/APO/AP_RcChannel.h
View File

@ -14,7 +14,7 @@
namespace apo {
enum rcMode_t {
RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT
RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT
};
/// @class AP_RcChannel
@ -23,60 +23,60 @@ 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);
/// 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;
// 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();
}
// 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);
}
// 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;
// configuration
APM_RC_Class & _rc;
// internal states
uint16_t _pwm; // this is the internal state, position is just created when needed
// 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);
// private methods
uint16_t _positionToPwm(const float & position);
float _pwmToPosition(const uint16_t & pwm);
};
} // apo

20
libraries/APO/AP_Var_keys.h
View File

@ -3,20 +3,20 @@
enum keys {
// general
k_config = 0,
k_cntrl,
k_guide,
k_sensorCalib,
// general
k_config = 0,
k_cntrl,
k_guide,
k_sensorCalib,
k_radioChannelsStart=10,
k_radioChannelsStart=10,
k_controllersStart=30,
k_controllersStart=30,
k_customStart=100,
k_customStart=100,
// 200-256 reserved for commands
k_commands = 200
// 200-256 reserved for commands
k_commands = 200
};
// max 256 keys

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

@ -1,7 +1,7 @@
/*
AnalogReadSerial
Reads an analog input on pin 0, prints the result to the serial monitor
Reads an analog input on pin 0, prints the result to the serial monitor
This example code is in the public domain.
*/
@ -10,40 +10,40 @@
int packetDrops = 0;
void handleMessage(mavlink_message_t * msg) {
Serial.print(", received mavlink message: ");
Serial.print(msg->msgid, DEC);
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;
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");
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;
// 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);
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);
}
// 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;
// Update packet drops counter
packetDrops += status.packet_rx_drop_count;
Serial.print(", dropped packets: ");
Serial.println(packetDrops);
delay(1000);
Serial.print(", dropped packets: ");
Serial.println(packetDrops);
delay(1000);
}

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

@ -19,91 +19,91 @@ 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;
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));
ch.push_back(
new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
1500, 1900));
ch.push_back(
new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
1900));
RadioTest() :
testPosition(2), testSign(1) {
ch.push_back(
new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
1500, 1900));
ch.push_back(
new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
1900));
Serial.begin(115200);
delay(2000);
Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization
delay(2000);
}
Serial.begin(115200);
delay(2000);
Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization
delay(2000);
}
void update() {
// read the radio
for (uint8_t i = 0; i < ch.getSize(); i++)
ch[i]->setUsingRadio();
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 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 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();
// 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);
}
delay(500);
}
};
RadioTest * test;
void setup() {
test = new RadioTest;
test = new RadioTest;
}
void loop() {
test->update();
test->update();
}

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

@ -19,107 +19,107 @@ 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;
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));
ch.push_back(
new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
1500, 1900));
ch.push_back(
new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
1900));
RadioTest() :
testPosition(2), testSign(1) {
ch.push_back(
new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100,
1500, 1900));
ch.push_back(
new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500,
1900));
ch.push_back(
new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500,
1900));
Serial.begin(115200);
delay(2000);
Serial.println("ArduPilot RC Channel test");
APM_RC.Init(); // APM Radio initialization
delay(2000);
}
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;
}
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);
// 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 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 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 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();
// 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);
}
delay(500);
}
};
RadioTest * test;
void setup() {
test = new RadioTest;
test = new RadioTest;
}
void loop() {
test->update();
test->update();
}
// vim:ts=4:sw=4:expandtab