mirror of https://github.com/ArduPilot/ardupilot
Added battery monitoring, arming to apo.
Arming added for quadrotor. Need to add to rover still. Battery monitoring added with auto shut-off in quadrotor controller. Finally split apo header and source files to allow faster compiling/ fix cyclic header inclusions.
This commit is contained in:
parent
35268cf6b9
commit
32af63f734
|
@ -9,6 +9,7 @@
|
|||
#define CONTROLLERQUAD_H_
|
||||
|
||||
#include "../APO/AP_Controller.h"
|
||||
#include "../APO/AP_BatteryMonitor.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
|
@ -61,22 +62,24 @@ public:
|
|||
AP_Controller(nav, guide, hal),
|
||||
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_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_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_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_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_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_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
|
||||
_armingClock(0), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
|
||||
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) {
|
||||
/*
|
||||
* allocate radio channels
|
||||
* the order of the channels has to match the enumeration above
|
||||
|
@ -112,138 +115,203 @@ public:
|
|||
}
|
||||
|
||||
virtual void update(const float & dt) {
|
||||
//_hal->debug->printf_P(PSTR("thr: %f, yaw: %f\n"),_hal->rc[CH_THRUST]->getRadioPosition(),_hal->rc[CH_YAW]->getRadioPosition());
|
||||
|
||||
// check for heartbeat
|
||||
// arming
|
||||
//
|
||||
// to arm: put stick to bottom right for 100 controller cycles
|
||||
// (max yaw, min throttle)
|
||||
//
|
||||
// didn't use clock here in case of millis() roll over
|
||||
// for long runs
|
||||
if ( (_hal->getState() != MAV_STATE_ACTIVE) &
|
||||
(_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) &&
|
||||
(_hal->rc[CH_YAW]->getRadioPosition() < -0.9) ) {
|
||||
|
||||
// always start clock at 0
|
||||
if (_armingClock<0) _armingClock = 0;
|
||||
|
||||
if (_armingClock++ >= 100) {
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
|
||||
_hal->setState(MAV_STATE_ACTIVE);
|
||||
} else {
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
|
||||
}
|
||||
}
|
||||
// disarming
|
||||
//
|
||||
// to disarm: put stick to bottom left for 100 controller cycles
|
||||
// (min yaw, min throttle)
|
||||
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &
|
||||
(_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) &&
|
||||
(_hal->rc[CH_YAW]->getRadioPosition() > 0.9) ) {
|
||||
|
||||
// always start clock at 0
|
||||
if (_armingClock>0) _armingClock = 0;
|
||||
|
||||
if (_armingClock-- <= -100) {
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
|
||||
_hal->setState(MAV_STATE_STANDBY);
|
||||
} else {
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
|
||||
}
|
||||
}
|
||||
// reset arming clock and report status
|
||||
else if (_armingClock != 0) {
|
||||
_armingClock = 0;
|
||||
if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
|
||||
else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
|
||||
}
|
||||
|
||||
// determine flight mode
|
||||
//
|
||||
// check for heartbeat from gcs, if not found go to failsafe
|
||||
if (_hal->heartBeatLost()) {
|
||||
_mode = MAV_MODE_FAILSAFE;
|
||||
setAllRadioChannelsToNeutral();
|
||||
_hal->setState(MAV_STATE_EMERGENCY);
|
||||
_hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n"));
|
||||
return;
|
||||
// if throttle less than 5% cut motor power
|
||||
} else if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.05) {
|
||||
_mode = MAV_MODE_LOCKED;
|
||||
setAllRadioChannelsToNeutral();
|
||||
_hal->setState(MAV_STATE_STANDBY);
|
||||
return;
|
||||
// if in live mode then set state to active
|
||||
} else if (_hal->getMode() == MODE_LIVE) {
|
||||
_hal->setState(MAV_STATE_ACTIVE);
|
||||
// if in hardware in the loop (control) mode, set to hilsim
|
||||
} else if (_hal->getMode() == MODE_HIL_CNTL) {
|
||||
_hal->setState(MAV_STATE_HILSIM);
|
||||
}
|
||||
|
||||
// manual mode
|
||||
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) {
|
||||
_mode = MAV_MODE_MANUAL;
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
|
||||
// if battery less than 5%, go to failsafe
|
||||
} else if (_hal->batteryMonitor->getPercentage() < 5) {
|
||||
_mode = MAV_MODE_FAILSAFE;
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
|
||||
// manual/auto switch
|
||||
} else {
|
||||
_mode = MAV_MODE_AUTO;
|
||||
// if all emergencies cleared, fall back to standby
|
||||
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
|
||||
if (_hal->rc[CH_MODE]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
|
||||
else _mode = MAV_MODE_AUTO;
|
||||
}
|
||||
|
||||
// commands for inner loop
|
||||
float cmdRoll = 0;
|
||||
float cmdPitch = 0;
|
||||
float cmdYawRate = 0;
|
||||
float thrustMix = 0;
|
||||
|
||||
// handle flight modes
|
||||
switch(_mode) {
|
||||
|
||||
case MAV_MODE_MANUAL: {
|
||||
setAllRadioChannelsManually();
|
||||
// "mix manual"
|
||||
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();
|
||||
break;
|
||||
case MAV_MODE_LOCKED: {
|
||||
_hal->setState(MAV_STATE_STANDBY);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MODE_FAILSAFE: {
|
||||
_hal->setState(MAV_STATE_EMERGENCY);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MODE_MANUAL: {
|
||||
manualPositionLoop();
|
||||
autoAttitudeLoop(dt);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MODE_AUTO: {
|
||||
// until position loop is tested just
|
||||
// go to standby
|
||||
_hal->setState(MAV_STATE_STANDBY);
|
||||
|
||||
//attitudeLoop();
|
||||
// XXX autoPositionLoop NOT TESTED, don't uncomment yet
|
||||
//autoPositionLoop(dt);
|
||||
//autoAttitudeLoop(dt);
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
|
||||
_hal->setState(MAV_STATE_EMERGENCY);
|
||||
}
|
||||
}
|
||||
|
||||
case MAV_MODE_AUTO: {
|
||||
|
||||
// XXX kills all commands,
|
||||
// auto not currently implemented
|
||||
setAllRadioChannelsToNeutral();
|
||||
|
||||
// position loop
|
||||
/*
|
||||
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(-yaw);
|
||||
float trigCos = cos(-yaw);
|
||||
_cmdPitch = _cmdEastTilt * trigCos
|
||||
- _cmdNorthTilt * trigSin;
|
||||
_cmdRoll = -_cmdEastTilt * trigSin
|
||||
+ _cmdNorthTilt * trigCos;
|
||||
// note that the north tilt is negative of the pitch
|
||||
}
|
||||
|
||||
//thrustMix += THRUST_HOVER_OFFSET;
|
||||
|
||||
// "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);
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// attitude loop
|
||||
float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),
|
||||
_nav->getRollRate(), dt);
|
||||
float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),
|
||||
_nav->getPitchRate(), dt);
|
||||
float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(), dt);
|
||||
|
||||
_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);
|
||||
|
||||
//_hal->debug->printf("R: %f\t L: %f\t F: %f\t B: %f\n",
|
||||
//_hal->rc[CH_RIGHT]->getPosition(),
|
||||
//_hal->rc[CH_LEFT]->getPosition(),
|
||||
//_hal->rc[CH_FRONT]->getPosition(),
|
||||
//_hal->rc[CH_BACK]->getPosition());
|
||||
|
||||
//_hal->debug->printf(
|
||||
// "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n",
|
||||
// rollMix, pitchMix, yawMix, thrustMix);
|
||||
|
||||
//_hal->debug->printf("cmdRoll: %f\t roll: %f\t rollMix: %f\n",
|
||||
// cmdRoll, _nav->getRoll(), rollMix);
|
||||
//_hal->debug->printf("cmdPitch: %f\t pitch: %f\t pitchMix: %f\n",
|
||||
// cmdPitch, _nav->getPitch(), pitchMix);
|
||||
//_hal->debug->printf("cmdYawRate: %f\t yawRate: %f\t yawMix: %f\n",
|
||||
// cmdYawRate, _nav->getYawRate(), yawMix);
|
||||
|
||||
//_hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n",
|
||||
//_hal->rc[CH_ROLL]->getRadioPwm(),
|
||||
//_hal->rc[CH_PITCH]->getRadioPwm(),
|
||||
//_hal->rc[CH_YAW]->getRadioPwm(),
|
||||
//_hal->rc[CH_THRUST]->getRadioPwm());
|
||||
// this sends commands to motors
|
||||
setMotors();
|
||||
}
|
||||
|
||||
virtual MAV_MODE getMode() {
|
||||
return (MAV_MODE) _mode.get();
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
AP_Uint8 _mode;
|
||||
BlockPIDDfb pidRoll, pidPitch, pidYaw;
|
||||
BlockPID pidYawRate;
|
||||
BlockPIDDfb pidPN, pidPE, pidPD;
|
||||
int32_t _armingClock;
|
||||
|
||||
float _thrustMix, _pitchMix, _rollMix, _yawMix;
|
||||
float _cmdRoll, _cmdPitch, _cmdYawRate;
|
||||
|
||||
void manualPositionLoop() {
|
||||
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();
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
_thrustMix = THRUST_HOVER_OFFSET + cmdDown;
|
||||
|
||||
// "thrust-trim-adjust"
|
||||
if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
|
||||
else _thrustMix /= cos(_cmdRoll);
|
||||
|
||||
if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
|
||||
else _thrustMix /= cos(_cmdPitch);
|
||||
}
|
||||
|
||||
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() {
|
||||
|
||||
switch (_hal->getState()) {
|
||||
|
||||
case MAV_STATE_ACTIVE: {
|
||||
digitalWrite(_hal->aLedPin, HIGH);
|
||||
// turn all motors off if below 0.1 throttle
|
||||
if (_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
|
||||
|
|
|
@ -39,6 +39,13 @@ static const bool compassEnabled = true;
|
|||
static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD;
|
||||
// compass orientation: See AP_Compass_HMC5843.h for possible values
|
||||
|
||||
// battery monitoring
|
||||
static const bool batteryMonitorEnabled = true;
|
||||
static const uint8_t batteryPin = 0;
|
||||
static const float batteryVoltageDivRatio = 6;
|
||||
static const float batteryMinVolt = 10.0;
|
||||
static const float batteryMaxVolt = 12.4;
|
||||
|
||||
static const bool rangeFinderFrontEnabled = false;
|
||||
static const bool rangeFinderBackEnabled = false;
|
||||
static const bool rangeFinderLeftEnabled = false;
|
||||
|
@ -47,11 +54,11 @@ static const bool rangeFinderUpEnabled = false;
|
|||
static const bool rangeFinderDownEnabled = false;
|
||||
|
||||
// loop rates
|
||||
static const float loop0Rate = 200; // attitude nav
|
||||
static const float loop1Rate = 50; // controller
|
||||
static const float loop2Rate = 10; // pos nav/ gcs fast
|
||||
static const float loop3Rate = 1; // gcs slow
|
||||
static const float loop4Rate = 0.1;
|
||||
static const float loopRate = 150; // attitude nav
|
||||
static const float loop0Rate = 50; // controller
|
||||
static const float loop1Rate = 5; // pos nav/ gcs fast
|
||||
static const float loop2Rate = 1; // gcs slow
|
||||
static const float loop3Rate = 0.1;
|
||||
|
||||
// position control loop
|
||||
static const float PID_POS_P = 0;
|
||||
|
@ -64,13 +71,15 @@ static const float PID_POS_Z_I = 0;
|
|||
static const float PID_POS_Z_D = 0;
|
||||
static const float PID_POS_Z_LIM = 0;
|
||||
static const float PID_POS_Z_AWU = 0;
|
||||
static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz
|
||||
|
||||
// attitude control loop
|
||||
static const float PID_ATT_P = 0.3;
|
||||
static const float PID_ATT_P = 0.17;
|
||||
static const float PID_ATT_I = 0.5;
|
||||
static const float PID_ATT_D = 0.08;
|
||||
static const float PID_ATT_LIM = 0.1; // 10 %
|
||||
static const float PID_ATT_AWU = 0.03; // 3 %
|
||||
static const float PID_ATT_D = 0.06;
|
||||
static const float PID_ATT_LIM = 0.05; // 10 %
|
||||
static const float PID_ATT_AWU = 0.005; // 0.5 %
|
||||
static const float PID_ATT_DFCUT = 25; // cut derivative feedback at 25 hz
|
||||
static const float PID_YAWPOS_P = 0;
|
||||
static const float PID_YAWPOS_I = 0;
|
||||
static const float PID_YAWPOS_D = 0;
|
||||
|
|
|
@ -46,6 +46,10 @@ void setup() {
|
|||
hal->adc = new ADC_CLASS;
|
||||
hal->adc->Init();
|
||||
|
||||
if (batteryMonitorEnabled) {
|
||||
hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt);
|
||||
}
|
||||
|
||||
if (gpsEnabled) {
|
||||
Serial1.begin(gpsBaud, 128, 16); // gps
|
||||
hal->debug->println_P(PSTR("initializing gps"));
|
||||
|
@ -167,7 +171,7 @@ void setup() {
|
|||
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
|
||||
|
||||
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
|
||||
loop0Rate, loop1Rate, loop2Rate, loop3Rate);
|
||||
loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
|
|
@ -6,6 +6,7 @@
|
|||
*/
|
||||
|
||||
#include "AP_Autopilot.h"
|
||||
#include "AP_BatteryMonitor.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
|
@ -13,16 +14,21 @@ class AP_HardwareAbstractionLayer;
|
|||
|
||||
AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
|
||||
float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) :
|
||||
Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide),
|
||||
_controller(controller), _hal(hal), _loop0Rate(loop0Rate),
|
||||
_loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate),
|
||||
_loop4Rate(loop3Rate), callback0Calls(0), clockInit(millis()) {
|
||||
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);
|
||||
|
||||
/*
|
||||
* Radio setup
|
||||
*/
|
||||
hal->debug->println_P(PSTR("initializing radio"));
|
||||
APM_RC.Init(); // APM Radio initialization,
|
||||
|
||||
/*
|
||||
* Calibration
|
||||
*/
|
||||
|
@ -69,7 +75,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
|
|||
}
|
||||
hal->debug->println_P(PSTR("waiting for hil packet"));
|
||||
}
|
||||
delay(1000);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
AP_MavlinkCommand::home.setAlt(_navigator->getAlt());
|
||||
|
@ -91,46 +97,31 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
|
|||
* Attach loops
|
||||
*/
|
||||
hal->debug->println_P(PSTR("attaching loops"));
|
||||
subLoops().push_back(new Loop(getLoopRate(1), callback1, this));
|
||||
subLoops().push_back(new Loop(getLoopRate(2), callback2, this));
|
||||
subLoops().push_back(new Loop(getLoopRate(3), callback3, this));
|
||||
subLoops().push_back(new Loop(getLoopRate(4), callback4, this));
|
||||
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"));
|
||||
|
||||
if (hal->getMode() == MODE_LIVE) {
|
||||
hal->setState(MAV_STATE_ACTIVE);
|
||||
} else {
|
||||
hal->setState(MAV_STATE_HILSIM);
|
||||
}
|
||||
|
||||
/*
|
||||
* Radio setup
|
||||
*/
|
||||
hal->debug->println_P(PSTR("initializing radio"));
|
||||
APM_RC.Init(); // APM Radio initialization,
|
||||
// start this after control loop is running
|
||||
|
||||
clockInit = millis();
|
||||
hal->setState(MAV_STATE_STANDBY);
|
||||
}
|
||||
|
||||
void AP_Autopilot::callback0(void * data) {
|
||||
void AP_Autopilot::callback(void * data) {
|
||||
AP_Autopilot * apo = (AP_Autopilot *) data;
|
||||
//apo->hal()->debug->println_P(PSTR("callback 0"));
|
||||
//apo->hal()->debug->println_P(PSTR("callback"));
|
||||
|
||||
/*
|
||||
* ahrs update
|
||||
*/
|
||||
|
||||
apo->callback0Calls++;
|
||||
apo->callbackCalls++;
|
||||
if (apo->getNavigator())
|
||||
apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0));
|
||||
apo->getNavigator()->updateFast(apo->dt());
|
||||
}
|
||||
|
||||
void AP_Autopilot::callback1(void * data) {
|
||||
void AP_Autopilot::callback0(void * data) {
|
||||
AP_Autopilot * apo = (AP_Autopilot *) data;
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 1"));
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 0"));
|
||||
|
||||
/*
|
||||
* hardware in the loop
|
||||
|
@ -145,7 +136,7 @@ void AP_Autopilot::callback1(void * data) {
|
|||
*/
|
||||
if (apo->getController()) {
|
||||
//apo->getHal()->debug->println_P(PSTR("updating controller"));
|
||||
apo->getController()->update(1. / apo->getLoopRate(1));
|
||||
apo->getController()->update(apo->subLoops()[0]->dt());
|
||||
}
|
||||
/*
|
||||
char msg[50];
|
||||
|
@ -154,9 +145,9 @@ void AP_Autopilot::callback1(void * data) {
|
|||
*/
|
||||
}
|
||||
|
||||
void AP_Autopilot::callback2(void * data) {
|
||||
void AP_Autopilot::callback1(void * data) {
|
||||
AP_Autopilot * apo = (AP_Autopilot *) data;
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 2"));
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 1"));
|
||||
|
||||
/*
|
||||
* update guidance laws
|
||||
|
@ -171,7 +162,7 @@ void AP_Autopilot::callback2(void * data) {
|
|||
* slow navigation loop update
|
||||
*/
|
||||
if (apo->getNavigator()) {
|
||||
apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
|
||||
apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt());
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -210,9 +201,9 @@ void AP_Autopilot::callback2(void * data) {
|
|||
*/
|
||||
}
|
||||
|
||||
void AP_Autopilot::callback3(void * data) {
|
||||
void AP_Autopilot::callback2(void * data) {
|
||||
AP_Autopilot * apo = (AP_Autopilot *) data;
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 3"));
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 2"));
|
||||
|
||||
/*
|
||||
* send telemetry
|
||||
|
@ -226,6 +217,11 @@ void AP_Autopilot::callback3(void * data) {
|
|||
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
|
||||
}
|
||||
|
||||
/*
|
||||
* update battery monitor
|
||||
*/
|
||||
if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update();
|
||||
|
||||
/*
|
||||
* send heartbeat
|
||||
*/
|
||||
|
@ -235,10 +231,10 @@ void AP_Autopilot::callback3(void * data) {
|
|||
* load/loop rate/ram debug
|
||||
*/
|
||||
apo->getHal()->load = apo->load();
|
||||
apo->getHal()->debug->printf_P(PSTR("missed calls: %d\n"),uint16_t(millis()*apo->getLoopRate(0)/1000-apo->callback0Calls));
|
||||
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);
|
||||
|
||||
/*
|
||||
|
@ -250,9 +246,9 @@ void AP_Autopilot::callback3(void * data) {
|
|||
//apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8));
|
||||
}
|
||||
|
||||
void AP_Autopilot::callback4(void * data) {
|
||||
void AP_Autopilot::callback3(void * data) {
|
||||
//AP_Autopilot * apo = (AP_Autopilot *) data;
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 4"));
|
||||
//apo->getHal()->debug->println_P(PSTR("callback 3"));
|
||||
}
|
||||
|
||||
} // apo
|
||||
|
|
|
@ -70,7 +70,7 @@ public:
|
|||
*/
|
||||
AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
|
||||
float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate);
|
||||
float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate);
|
||||
|
||||
/**
|
||||
* Accessors
|
||||
|
@ -88,68 +88,51 @@ public:
|
|||
return _hal;
|
||||
}
|
||||
|
||||
float getLoopRate(uint8_t i) {
|
||||
switch(i) {
|
||||
case 0: return _loop0Rate;
|
||||
case 1: return _loop1Rate;
|
||||
case 2: return _loop2Rate;
|
||||
case 3: return _loop3Rate;
|
||||
case 4: return _loop4Rate;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Loop Monitoring
|
||||
*/
|
||||
uint32_t callback0Calls;
|
||||
uint32_t clockInit;
|
||||
uint32_t callbackCalls;
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Loop 0 Callbacks (fastest)
|
||||
* 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);
|
||||
float _loop0Rate;
|
||||
|
||||
/**
|
||||
* Loop 1 Callbacks
|
||||
* - control
|
||||
* - compass reading
|
||||
* @see callback0
|
||||
* - gps sensor fusion
|
||||
* - compass sensor fusion
|
||||
* @see callback
|
||||
*/
|
||||
static void callback1(void * data);
|
||||
float _loop1Rate;
|
||||
|
||||
/**
|
||||
* Loop 2 Callbacks
|
||||
* - gps sensor fusion
|
||||
* - compass sensor fusion
|
||||
* @see callback0
|
||||
* - slow messages
|
||||
* @see callback
|
||||
*/
|
||||
static void callback2(void * data);
|
||||
float _loop2Rate;
|
||||
|
||||
/**
|
||||
* Loop 3 Callbacks
|
||||
* - slow messages
|
||||
* @see callback0
|
||||
*/
|
||||
static void callback3(void * data);
|
||||
float _loop3Rate;
|
||||
|
||||
/**
|
||||
* Loop 4 Callbacks
|
||||
* - super slow messages
|
||||
* - log writing
|
||||
* @see callback0
|
||||
* @see callback
|
||||
*/
|
||||
static void callback4(void * data);
|
||||
float _loop4Rate;
|
||||
static void callback3(void * data);
|
||||
|
||||
/**
|
||||
* Components
|
||||
|
|
|
@ -0,0 +1,10 @@
|
|||
/*
|
||||
* AP_BatteryMonitor.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_BatteryMonitor.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
} // apo
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* AP_BatteryMonitor.h
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AP_BATTERYMONITOR_H_
|
||||
#define AP_BATTERYMONITOR_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <wiring.h>
|
||||
|
||||
namespace apo {
|
||||
|
||||
class AP_BatteryMonitor {
|
||||
public:
|
||||
AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) :
|
||||
_pin(pin), _voltageDivRatio(voltageDivRatio),
|
||||
_minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) {
|
||||
}
|
||||
|
||||
void update() {
|
||||
// low pass filter on voltage
|
||||
_voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Accessors
|
||||
*/
|
||||
float getVoltage() {
|
||||
return _voltage;
|
||||
}
|
||||
|
||||
uint8_t getPercentage() {
|
||||
float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt);
|
||||
if (norm < 0) norm = 0;
|
||||
else if (norm > 1) norm = 1;
|
||||
return 100*norm;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
uint8_t _pin;
|
||||
float _voltageDivRatio;
|
||||
float _voltage;
|
||||
float _minVolt;
|
||||
float _maxVolt;
|
||||
};
|
||||
|
||||
} // namespace apo
|
||||
|
||||
#endif /* AP_BATTERYMONITOR_H_ */
|
|
@ -5,11 +5,717 @@
|
|||
* Author: jgoppert
|
||||
*/
|
||||
|
||||
#include "../FastSerial/FastSerial.h"
|
||||
#include "AP_CommLink.h"
|
||||
#include "AP_Navigator.h"
|
||||
#include "AP_Guide.h"
|
||||
#include "AP_Controller.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "AP_RcChannel.h"
|
||||
#include "../AP_GPS/AP_GPS.h"
|
||||
#include "../AP_Math/AP_Math.h"
|
||||
#include "../AP_IMU/AP_IMU.h"
|
||||
#include "../AP_Compass/AP_Compass.h"
|
||||
#include "AP_BatteryMonitor.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
uint8_t MavlinkComm::_nChannels = 0;
|
||||
uint8_t MavlinkComm::_paramNameLengthMax = 13;
|
||||
|
||||
AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
|
||||
_link(link), _navigator(navigator), _guide(guide),
|
||||
_controller(controller), _hal(hal) {
|
||||
}
|
||||
|
||||
MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
|
||||
AP_CommLink(link, nav, guide, controller, hal),
|
||||
|
||||
// options
|
||||
_useRelativeAlt(true),
|
||||
|
||||
// commands
|
||||
_sendingCmds(false), _receivingCmds(false),
|
||||
_cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()),
|
||||
_cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0),
|
||||
_cmdMax(30), _cmdNumberRequested(0),
|
||||
|
||||
// parameters
|
||||
_parameterCount(0), _queuedParameter(NULL),
|
||||
_queuedParameterIndex(0) {
|
||||
|
||||
switch (_nChannels) {
|
||||
case 0:
|
||||
mavlink_comm_0_port = link;
|
||||
_channel = MAVLINK_COMM_0;
|
||||
_nChannels++;
|
||||
break;
|
||||
case 1:
|
||||
mavlink_comm_1_port = link;
|
||||
_channel = MAVLINK_COMM_1;
|
||||
_nChannels++;
|
||||
break;
|
||||
default:
|
||||
// signal that number of channels exceeded
|
||||
_channel = MAVLINK_COMM_3;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkComm::send() {
|
||||
// if number of channels exceeded return
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
}
|
||||
|
||||
void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
|
||||
//_hal->debug->printf_P(PSTR("send message\n"));
|
||||
|
||||
// if number of channels exceeded return
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
|
||||
uint64_t timeStamp = micros();
|
||||
|
||||
switch (id) {
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: {
|
||||
mavlink_msg_heartbeat_send(_channel, mavlink_system.type,
|
||||
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE: {
|
||||
mavlink_msg_attitude_send(_channel, timeStamp,
|
||||
_navigator->getRoll(), _navigator->getPitch(),
|
||||
_navigator->getYaw(), _navigator->getRollRate(),
|
||||
_navigator->getPitchRate(), _navigator->getYawRate());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION: {
|
||||
mavlink_msg_global_position_send(_channel, timeStamp,
|
||||
_navigator->getLat() * rad2Deg,
|
||||
_navigator->getLon() * rad2Deg, _navigator->getAlt(),
|
||||
_navigator->getVN(), _navigator->getVE(),
|
||||
_navigator->getVD());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GPS_RAW: {
|
||||
mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
|
||||
_navigator->getLat() * rad2Deg,
|
||||
_navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
|
||||
_navigator->getGroundSpeed(),
|
||||
_navigator->getYaw() * rad2Deg);
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: {
|
||||
mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(),
|
||||
_navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0,
|
||||
_navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg);
|
||||
break;
|
||||
}
|
||||
*/
|
||||
|
||||
case MAVLINK_MSG_ID_SCALED_IMU: {
|
||||
/*
|
||||
* 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);
|
||||
*/
|
||||
Vector3f accel = _hal->imu->get_accel();
|
||||
Vector3f gyro = _hal->imu->get_gyro();
|
||||
mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x,
|
||||
1000 * accel.y, 1000 * accel.z, 1000 * gyro.x,
|
||||
1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x,
|
||||
_hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
|
||||
int16_t ch[8];
|
||||
for (int i = 0; i < 8; i++)
|
||||
ch[i] = 0;
|
||||
for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) {
|
||||
ch[i] = 10000 * _hal->rc[i]->getPosition();
|
||||
//_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
|
||||
}
|
||||
mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
|
||||
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
|
||||
int16_t ch[8];
|
||||
for (int i = 0; i < 8; i++)
|
||||
ch[i] = 0;
|
||||
for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++)
|
||||
ch[i] = _hal->rc[i]->getPwm();
|
||||
mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
|
||||
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_SYS_STATUS: {
|
||||
|
||||
uint16_t batteryVoltage = 0; // (milli volts)
|
||||
uint16_t batteryPercentage = 1000; // times 10
|
||||
if (_hal->batteryMonitor) {
|
||||
batteryPercentage = _hal->batteryMonitor->getPercentage()*10;
|
||||
batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
|
||||
}
|
||||
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
|
||||
_guide->getMode(), _hal->getState(), _hal->load * 10,
|
||||
batteryVoltage, batteryPercentage, _packetDrops);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
|
||||
//mavlink_waypoint_ack_t packet;
|
||||
uint8_t type = 0; // ok (0), error(1)
|
||||
mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
|
||||
_cmdDestCompId, type);
|
||||
|
||||
// turn off waypoint send
|
||||
_receivingCmds = false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
|
||||
mavlink_msg_waypoint_current_send(_channel,
|
||||
_guide->getCurrentIndex());
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
char msg[50];
|
||||
sprintf(msg, "autopilot sending unknown command with id: %d", id);
|
||||
sendText(SEVERITY_HIGH, msg);
|
||||
}
|
||||
|
||||
} // switch
|
||||
} // send message
|
||||
|
||||
void MavlinkComm::receive() {
|
||||
//_hal->debug->printf_P(PSTR("receive\n"));
|
||||
// if number of channels exceeded return
|
||||
//
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
|
||||
// receive new packets
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
status.packet_rx_drop_count = 0;
|
||||
|
||||
// process received bytes
|
||||
while (comm_get_available(_channel)) {
|
||||
uint8_t c = comm_receive_ch(_channel);
|
||||
|
||||
// Try to get a new message
|
||||
if (mavlink_parse_char(_channel, c, &msg, &status))
|
||||
_handleMessage(&msg);
|
||||
}
|
||||
|
||||
// Update packet drops counter
|
||||
_packetDrops += status.packet_rx_drop_count;
|
||||
}
|
||||
|
||||
void MavlinkComm::sendText(uint8_t severity, const char *str) {
|
||||
mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
|
||||
}
|
||||
|
||||
void MavlinkComm::sendText(uint8_t severity, const prog_char_t *str) {
|
||||
mavlink_statustext_t m;
|
||||
uint8_t i;
|
||||
for (i = 0; i < sizeof(m.text); i++) {
|
||||
m.text[i] = pgm_read_byte((const prog_char *) (str++));
|
||||
}
|
||||
if (i < sizeof(m.text))
|
||||
m.text[i] = 0;
|
||||
sendText(severity, (const char *) m.text);
|
||||
}
|
||||
|
||||
void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {
|
||||
}
|
||||
|
||||
/**
|
||||
* sends parameters one at a time
|
||||
*/
|
||||
void MavlinkComm::sendParameters() {
|
||||
//_hal->debug->printf_P(PSTR("send parameters\n"));
|
||||
// Check to see if we are sending parameters
|
||||
while (NULL != _queuedParameter) {
|
||||
AP_Var *vp;
|
||||
float value;
|
||||
|
||||
// copy the current parameter and prepare to move to the next
|
||||
vp = _queuedParameter;
|
||||
_queuedParameter = _queuedParameter->next();
|
||||
|
||||
// if the parameter can be cast to float, report it here and break out of the loop
|
||||
value = vp->cast_to_float();
|
||||
if (!isnan(value)) {
|
||||
|
||||
char paramName[_paramNameLengthMax];
|
||||
vp->copy_name(paramName, sizeof(paramName));
|
||||
|
||||
mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
|
||||
value, _countParameters(), _queuedParameterIndex);
|
||||
|
||||
_queuedParameterIndex++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* request commands one at a time
|
||||
*/
|
||||
void MavlinkComm::requestCmds() {
|
||||
//_hal->debug->printf_P(PSTR("requesting commands\n"));
|
||||
// request cmds one by one
|
||||
if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) {
|
||||
mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId,
|
||||
_cmdDestCompId, _cmdRequestIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
|
||||
|
||||
uint32_t timeStamp = micros();
|
||||
|
||||
switch (msg->msgid) {
|
||||
_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: {
|
||||
mavlink_heartbeat_t packet;
|
||||
mavlink_msg_heartbeat_decode(msg, &packet);
|
||||
_hal->lastHeartBeat = micros();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GPS_RAW: {
|
||||
// decode
|
||||
mavlink_gps_raw_t packet;
|
||||
mavlink_msg_gps_raw_decode(msg, &packet);
|
||||
|
||||
_navigator->setTimeStamp(timeStamp);
|
||||
_navigator->setLat(packet.lat * deg2Rad);
|
||||
_navigator->setLon(packet.lon * deg2Rad);
|
||||
_navigator->setAlt(packet.alt);
|
||||
_navigator->setYaw(packet.hdg * deg2Rad);
|
||||
_navigator->setGroundSpeed(packet.v);
|
||||
_navigator->setAirSpeed(packet.v);
|
||||
//_hal->debug->printf_P(PSTR("received hil gps raw packet\n"));
|
||||
/*
|
||||
_hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
|
||||
packet.lat,
|
||||
packet.lon,
|
||||
packet.alt);
|
||||
*/
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE: {
|
||||
// decode
|
||||
mavlink_attitude_t packet;
|
||||
mavlink_msg_attitude_decode(msg, &packet);
|
||||
|
||||
// set dcm hil sensor
|
||||
_navigator->setTimeStamp(timeStamp);
|
||||
_navigator->setRoll(packet.roll);
|
||||
_navigator->setPitch(packet.pitch);
|
||||
_navigator->setYaw(packet.yaw);
|
||||
_navigator->setRollRate(packet.rollspeed);
|
||||
_navigator->setPitchRate(packet.pitchspeed);
|
||||
_navigator->setYawRate(packet.yawspeed);
|
||||
//_hal->debug->printf_P(PSTR("received hil attitude packet\n"));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ACTION: {
|
||||
// decode
|
||||
mavlink_action_t packet;
|
||||
mavlink_msg_action_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target, packet.target_component))
|
||||
break;
|
||||
|
||||
// do action
|
||||
sendText(SEVERITY_LOW, PSTR("action received"));
|
||||
switch (packet.action) {
|
||||
|
||||
case MAV_ACTION_STORAGE_READ:
|
||||
AP_Var::load_all();
|
||||
break;
|
||||
|
||||
case MAV_ACTION_STORAGE_WRITE:
|
||||
AP_Var::save_all();
|
||||
break;
|
||||
|
||||
case MAV_ACTION_CALIBRATE_RC:
|
||||
case MAV_ACTION_CALIBRATE_GYRO:
|
||||
case MAV_ACTION_CALIBRATE_MAG:
|
||||
case MAV_ACTION_CALIBRATE_ACC:
|
||||
case MAV_ACTION_CALIBRATE_PRESSURE:
|
||||
case MAV_ACTION_REBOOT:
|
||||
case MAV_ACTION_REC_START:
|
||||
case MAV_ACTION_REC_PAUSE:
|
||||
case MAV_ACTION_REC_STOP:
|
||||
case MAV_ACTION_TAKEOFF:
|
||||
case MAV_ACTION_LAND:
|
||||
case MAV_ACTION_NAVIGATE:
|
||||
case MAV_ACTION_LOITER:
|
||||
case MAV_ACTION_MOTORS_START:
|
||||
case MAV_ACTION_CONFIRM_KILL:
|
||||
case MAV_ACTION_EMCY_KILL:
|
||||
case MAV_ACTION_MOTORS_STOP:
|
||||
case MAV_ACTION_SHUTDOWN:
|
||||
case MAV_ACTION_CONTINUE:
|
||||
case MAV_ACTION_SET_MANUAL:
|
||||
case MAV_ACTION_SET_AUTO:
|
||||
case MAV_ACTION_LAUNCH:
|
||||
case MAV_ACTION_RETURN:
|
||||
case MAV_ACTION_EMCY_LAND:
|
||||
case MAV_ACTION_HALT:
|
||||
sendText(SEVERITY_LOW, PSTR("action not implemented"));
|
||||
break;
|
||||
default:
|
||||
sendText(SEVERITY_LOW, PSTR("unknown action"));
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint request list"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_request_list_t packet;
|
||||
mavlink_msg_waypoint_request_list_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// Start sending waypoints
|
||||
mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
|
||||
_guide->getNumberOfCommands());
|
||||
|
||||
_cmdTimeLastSent = millis();
|
||||
_cmdTimeLastReceived = millis();
|
||||
_sendingCmds = true;
|
||||
_receivingCmds = false;
|
||||
_cmdDestSysId = msg->sysid;
|
||||
_cmdDestCompId = msg->compid;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint request"));
|
||||
|
||||
// Check if sending waypiont
|
||||
if (!_sendingCmds)
|
||||
break;
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_request_t packet;
|
||||
mavlink_msg_waypoint_request_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
|
||||
AP_MavlinkCommand cmd(packet.seq);
|
||||
|
||||
mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
|
||||
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
|
||||
wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
|
||||
wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
|
||||
wp.z);
|
||||
|
||||
// update last waypoint comm stamp
|
||||
_cmdTimeLastSent = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_ack_t packet;
|
||||
mavlink_msg_waypoint_ack_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// check for error
|
||||
//uint8_t type = packet.type; // ok (0), error(1)
|
||||
|
||||
// turn off waypoint send
|
||||
_sendingCmds = false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
sendText(SEVERITY_LOW, PSTR("param request list"));
|
||||
|
||||
// decode
|
||||
mavlink_param_request_list_t packet;
|
||||
mavlink_msg_param_request_list_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// Start sending parameters - next call to ::update will kick the first one out
|
||||
|
||||
_queuedParameter = AP_Var::first();
|
||||
_queuedParameterIndex = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint clear all"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_clear_all_t packet;
|
||||
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// clear all waypoints
|
||||
uint8_t type = 0; // ok (0), error(1)
|
||||
_guide->setNumberOfCommands(1);
|
||||
_guide->setCurrentIndex(0);
|
||||
|
||||
// send acknowledgement 3 times to makes sure it is received
|
||||
for (int i = 0; i < 3; i++)
|
||||
mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
|
||||
msg->compid, type);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint set current"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_set_current_t packet;
|
||||
mavlink_msg_waypoint_set_current_decode(msg, &packet);
|
||||
Serial.print("Packet Sequence:");
|
||||
Serial.println(packet.seq);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// set current waypoint
|
||||
Serial.print("Current Index:");
|
||||
Serial.println(_guide->getCurrentIndex());
|
||||
Serial.flush();
|
||||
_guide->setCurrentIndex(packet.seq);
|
||||
mavlink_msg_waypoint_current_send(_channel,
|
||||
_guide->getCurrentIndex());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint count"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_count_t packet;
|
||||
mavlink_msg_waypoint_count_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// start waypoint receiving
|
||||
if (packet.count > _cmdMax) {
|
||||
packet.count = _cmdMax;
|
||||
}
|
||||
_cmdNumberRequested = packet.count;
|
||||
_cmdTimeLastReceived = millis();
|
||||
_receivingCmds = true;
|
||||
_sendingCmds = false;
|
||||
_cmdRequestIndex = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint"));
|
||||
|
||||
// Check if receiving waypiont
|
||||
if (!_receivingCmds) {
|
||||
//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
|
||||
break;
|
||||
}
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_t packet;
|
||||
mavlink_msg_waypoint_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// check if this is the requested waypoint
|
||||
if (packet.seq != _cmdRequestIndex) {
|
||||
char warningMsg[50];
|
||||
sprintf(warningMsg,
|
||||
"waypoint request out of sequence: (packet) %d / %d (ap)",
|
||||
packet.seq, _cmdRequestIndex);
|
||||
sendText(SEVERITY_HIGH, warningMsg);
|
||||
break;
|
||||
}
|
||||
|
||||
_hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
|
||||
packet.x,
|
||||
packet.y,
|
||||
packet.z);
|
||||
|
||||
// store waypoint
|
||||
AP_MavlinkCommand command(packet);
|
||||
//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
|
||||
_cmdRequestIndex++;
|
||||
if (_cmdRequestIndex == _cmdNumberRequested) {
|
||||
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
|
||||
_receivingCmds = false;
|
||||
_guide->setNumberOfCommands(_cmdNumberRequested);
|
||||
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
|
||||
} else if (_cmdRequestIndex > _cmdNumberRequested) {
|
||||
_receivingCmds = false;
|
||||
}
|
||||
_cmdTimeLastReceived = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET: {
|
||||
sendText(SEVERITY_LOW, PSTR("param set"));
|
||||
AP_Var *vp;
|
||||
AP_Meta_class::Type_id var_type;
|
||||
|
||||
// decode
|
||||
mavlink_param_set_t packet;
|
||||
mavlink_msg_param_set_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// set parameter
|
||||
|
||||
char key[_paramNameLengthMax + 1];
|
||||
strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
|
||||
key[_paramNameLengthMax] = 0;
|
||||
|
||||
// find the requested parameter
|
||||
vp = AP_Var::find(key);
|
||||
if ((NULL != vp) && // exists
|
||||
!isnan(packet.param_value) && // not nan
|
||||
!isinf(packet.param_value)) { // not inf
|
||||
|
||||
// add a small amount before casting parameter values
|
||||
// from float to integer to avoid truncating to the
|
||||
// next lower integer value.
|
||||
const float rounding_addition = 0.01;
|
||||
|
||||
// fetch the variable type ID
|
||||
var_type = vp->meta_type_id();
|
||||
|
||||
// handle variables with standard type IDs
|
||||
if (var_type == AP_Var::k_typeid_float) {
|
||||
((AP_Float *) vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_float16) {
|
||||
((AP_Float16 *) vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int32) {
|
||||
((AP_Int32 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int16) {
|
||||
((AP_Int16 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int8) {
|
||||
((AP_Int8 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
} else {
|
||||
// we don't support mavlink set on this parameter
|
||||
break;
|
||||
}
|
||||
|
||||
// Report back the new value if we accepted the change
|
||||
// we send the value we actually set, which could be
|
||||
// different from the value sent, in case someone sent
|
||||
// a fractional value to an integer type
|
||||
mavlink_msg_param_value_send(_channel, (int8_t *) key,
|
||||
vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
|
||||
}
|
||||
|
||||
break;
|
||||
} // end case
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t MavlinkComm::_countParameters() {
|
||||
// if we haven't cached the parameter count yet...
|
||||
if (0 == _parameterCount) {
|
||||
AP_Var *vp;
|
||||
|
||||
vp = AP_Var::first();
|
||||
do {
|
||||
// if a parameter responds to cast_to_float then we are going to be able to report it
|
||||
if (!isnan(vp->cast_to_float())) {
|
||||
_parameterCount++;
|
||||
}
|
||||
} while (NULL != (vp = vp->next()));
|
||||
}
|
||||
return _parameterCount;
|
||||
}
|
||||
|
||||
AP_Var * _findParameter(uint16_t index) {
|
||||
AP_Var *vp;
|
||||
|
||||
vp = AP_Var::first();
|
||||
while (NULL != vp) {
|
||||
|
||||
// if the parameter is reportable
|
||||
if (!(isnan(vp->cast_to_float()))) {
|
||||
// if we have counted down to the index we want
|
||||
if (0 == index) {
|
||||
// return the parameter
|
||||
return vp;
|
||||
}
|
||||
// count off this parameter, as it is reportable but not
|
||||
// the one we want
|
||||
index--;
|
||||
}
|
||||
// and move to the next parameter
|
||||
vp = vp->next();
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// check the target
|
||||
uint8_t MavlinkComm::_checkTarget(uint8_t sysid, uint8_t compid) {
|
||||
/*
|
||||
char msg[50];
|
||||
sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid,
|
||||
mavlink_system.sysid, compid, mavlink_system.compid);
|
||||
sendText(SEVERITY_LOW, msg);
|
||||
*/
|
||||
if (sysid != mavlink_system.sysid) {
|
||||
//sendText(SEVERITY_LOW, PSTR("system id mismatch"));
|
||||
return 1;
|
||||
|
||||
} else if (compid != mavlink_system.compid) {
|
||||
//sendText(SEVERITY_LOW, PSTR("component id mismatch"));
|
||||
return 0; // XXX currently not receiving correct compid from gcs
|
||||
|
||||
} else {
|
||||
return 0; // no error
|
||||
}
|
||||
}
|
||||
|
||||
} // apo
|
||||
|
|
|
@ -19,9 +19,12 @@
|
|||
#ifndef AP_CommLink_H
|
||||
#define AP_CommLink_H
|
||||
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include <inttypes.h>
|
||||
#include "../AP_Common/AP_Common.h"
|
||||
#include "../AP_Common/AP_Vector.h"
|
||||
#include "AP_MavlinkCommand.h"
|
||||
#include "AP_Controller.h"
|
||||
|
||||
class FastSerial;
|
||||
|
||||
namespace apo {
|
||||
|
||||
|
@ -43,10 +46,7 @@ class AP_CommLink {
|
|||
public:
|
||||
|
||||
AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
|
||||
_link(link), _navigator(navigator), _guide(guide),
|
||||
_controller(controller), _hal(hal) {
|
||||
}
|
||||
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;
|
||||
|
@ -67,264 +67,24 @@ protected:
|
|||
class MavlinkComm: public AP_CommLink {
|
||||
public:
|
||||
MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
|
||||
AP_CommLink(link, nav, guide, controller, hal),
|
||||
AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
|
||||
|
||||
// options
|
||||
_useRelativeAlt(true),
|
||||
|
||||
// commands
|
||||
_sendingCmds(false), _receivingCmds(false),
|
||||
_cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()),
|
||||
_cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0),
|
||||
_cmdMax(30), _cmdNumberRequested(0),
|
||||
|
||||
// parameters
|
||||
_parameterCount(0), _queuedParameter(NULL),
|
||||
_queuedParameterIndex(0) {
|
||||
|
||||
switch (_nChannels) {
|
||||
case 0:
|
||||
mavlink_comm_0_port = link;
|
||||
_channel = MAVLINK_COMM_0;
|
||||
_nChannels++;
|
||||
break;
|
||||
case 1:
|
||||
mavlink_comm_1_port = link;
|
||||
_channel = MAVLINK_COMM_1;
|
||||
_nChannels++;
|
||||
break;
|
||||
default:
|
||||
// signal that number of channels exceeded
|
||||
_channel = MAVLINK_COMM_3;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void send() {
|
||||
// if number of channels exceeded return
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
}
|
||||
|
||||
void sendMessage(uint8_t id, uint32_t param = 0) {
|
||||
//_hal->debug->printf_P(PSTR("send message\n"));
|
||||
|
||||
// if number of channels exceeded return
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
|
||||
uint64_t timeStamp = micros();
|
||||
|
||||
switch (id) {
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: {
|
||||
mavlink_msg_heartbeat_send(_channel, mavlink_system.type,
|
||||
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE: {
|
||||
mavlink_msg_attitude_send(_channel, timeStamp,
|
||||
_navigator->getRoll(), _navigator->getPitch(),
|
||||
_navigator->getYaw(), _navigator->getRollRate(),
|
||||
_navigator->getPitchRate(), _navigator->getYawRate());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION: {
|
||||
mavlink_msg_global_position_send(_channel, timeStamp,
|
||||
_navigator->getLat() * rad2Deg,
|
||||
_navigator->getLon() * rad2Deg, _navigator->getAlt(),
|
||||
_navigator->getVN(), _navigator->getVE(),
|
||||
_navigator->getVD());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GPS_RAW: {
|
||||
mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
|
||||
_navigator->getLat() * rad2Deg,
|
||||
_navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
|
||||
_navigator->getGroundSpeed(),
|
||||
_navigator->getYaw() * rad2Deg);
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: {
|
||||
mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(),
|
||||
_navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0,
|
||||
_navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg);
|
||||
break;
|
||||
}
|
||||
*/
|
||||
|
||||
case MAVLINK_MSG_ID_SCALED_IMU: {
|
||||
/*
|
||||
* 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);
|
||||
*/
|
||||
Vector3f accel = _hal->imu->get_accel();
|
||||
Vector3f gyro = _hal->imu->get_gyro();
|
||||
mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x,
|
||||
1000 * accel.y, 1000 * accel.z, 1000 * gyro.x,
|
||||
1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x,
|
||||
_hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
|
||||
int16_t ch[8];
|
||||
for (int i = 0; i < 8; i++)
|
||||
ch[i] = 0;
|
||||
for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) {
|
||||
ch[i] = 10000 * _hal->rc[i]->getPosition();
|
||||
//_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
|
||||
}
|
||||
mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
|
||||
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
|
||||
int16_t ch[8];
|
||||
for (int i = 0; i < 8; i++)
|
||||
ch[i] = 0;
|
||||
for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++)
|
||||
ch[i] = _hal->rc[i]->getPwm();
|
||||
mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
|
||||
ch[3], ch[4], ch[5], ch[6], ch[7], 255);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_SYS_STATUS: {
|
||||
|
||||
float batteryVoltage, temp;
|
||||
temp = analogRead(0);
|
||||
batteryVoltage = ((temp * 5 / 1023) / 0.28);
|
||||
|
||||
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
|
||||
_guide->getMode(), _hal->getState(), _hal->load * 10,
|
||||
batteryVoltage * 1000,
|
||||
(batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
|
||||
//mavlink_waypoint_ack_t packet;
|
||||
uint8_t type = 0; // ok (0), error(1)
|
||||
mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
|
||||
_cmdDestCompId, type);
|
||||
|
||||
// turn off waypoint send
|
||||
_receivingCmds = false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
|
||||
mavlink_msg_waypoint_current_send(_channel,
|
||||
_guide->getCurrentIndex());
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
char msg[50];
|
||||
sprintf(msg, "autopilot sending unknown command with id: %d", id);
|
||||
sendText(SEVERITY_HIGH, msg);
|
||||
}
|
||||
|
||||
} // switch
|
||||
} // send message
|
||||
|
||||
virtual void receive() {
|
||||
//_hal->debug->printf_P(PSTR("receive\n"));
|
||||
// if number of channels exceeded return
|
||||
//
|
||||
if (_channel == MAVLINK_COMM_3)
|
||||
return;
|
||||
|
||||
// receive new packets
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
|
||||
// process received bytes
|
||||
while (comm_get_available(_channel)) {
|
||||
uint8_t c = comm_receive_ch(_channel);
|
||||
|
||||
// Try to get a new message
|
||||
if (mavlink_parse_char(_channel, c, &msg, &status))
|
||||
_handleMessage(&msg);
|
||||
}
|
||||
|
||||
// Update packet drops counter
|
||||
_packetDrops += status.packet_rx_drop_count;
|
||||
}
|
||||
|
||||
void sendText(uint8_t severity, const char *str) {
|
||||
mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
|
||||
}
|
||||
|
||||
void sendText(uint8_t severity, const prog_char_t *str) {
|
||||
mavlink_statustext_t m;
|
||||
uint8_t i;
|
||||
for (i = 0; i < sizeof(m.text); i++) {
|
||||
m.text[i] = pgm_read_byte((const prog_char *) (str++));
|
||||
}
|
||||
if (i < sizeof(m.text))
|
||||
m.text[i] = 0;
|
||||
sendText(severity, (const char *) m.text);
|
||||
}
|
||||
|
||||
void 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() {
|
||||
//_hal->debug->printf_P(PSTR("send parameters\n"));
|
||||
// Check to see if we are sending parameters
|
||||
while (NULL != _queuedParameter) {
|
||||
AP_Var *vp;
|
||||
float value;
|
||||
|
||||
// copy the current parameter and prepare to move to the next
|
||||
vp = _queuedParameter;
|
||||
_queuedParameter = _queuedParameter->next();
|
||||
|
||||
// if the parameter can be cast to float, report it here and break out of the loop
|
||||
value = vp->cast_to_float();
|
||||
if (!isnan(value)) {
|
||||
|
||||
char paramName[_paramNameLengthMax];
|
||||
vp->copy_name(paramName, sizeof(paramName));
|
||||
|
||||
mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
|
||||
value, _countParameters(), _queuedParameterIndex);
|
||||
|
||||
_queuedParameterIndex++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
void sendParameters();
|
||||
|
||||
/**
|
||||
* request commands one at a time
|
||||
*/
|
||||
void requestCmds() {
|
||||
//_hal->debug->printf_P(PSTR("requesting commands\n"));
|
||||
// request cmds one by one
|
||||
if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) {
|
||||
mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId,
|
||||
_cmdDestCompId, _cmdRequestIndex);
|
||||
}
|
||||
}
|
||||
void requestCmds();
|
||||
|
||||
private:
|
||||
|
||||
|
@ -354,432 +114,14 @@ private:
|
|||
uint16_t _packetDrops;
|
||||
static uint8_t _nChannels;
|
||||
|
||||
void _handleMessage(mavlink_message_t * msg) {
|
||||
void _handleMessage(mavlink_message_t * msg);
|
||||
|
||||
uint32_t timeStamp = micros();
|
||||
uint16_t _countParameters();
|
||||
|
||||
switch (msg->msgid) {
|
||||
_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid);
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: {
|
||||
mavlink_heartbeat_t packet;
|
||||
mavlink_msg_heartbeat_decode(msg, &packet);
|
||||
_hal->lastHeartBeat = micros();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_GPS_RAW: {
|
||||
// decode
|
||||
mavlink_gps_raw_t packet;
|
||||
mavlink_msg_gps_raw_decode(msg, &packet);
|
||||
|
||||
_navigator->setTimeStamp(timeStamp);
|
||||
_navigator->setLat(packet.lat * deg2Rad);
|
||||
_navigator->setLon(packet.lon * deg2Rad);
|
||||
_navigator->setAlt(packet.alt);
|
||||
_navigator->setYaw(packet.hdg * deg2Rad);
|
||||
_navigator->setGroundSpeed(packet.v);
|
||||
_navigator->setAirSpeed(packet.v);
|
||||
//_hal->debug->printf_P(PSTR("received hil gps raw packet\n"));
|
||||
/*
|
||||
_hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
|
||||
packet.lat,
|
||||
packet.lon,
|
||||
packet.alt);
|
||||
*/
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE: {
|
||||
// decode
|
||||
mavlink_attitude_t packet;
|
||||
mavlink_msg_attitude_decode(msg, &packet);
|
||||
|
||||
// set dcm hil sensor
|
||||
_navigator->setTimeStamp(timeStamp);
|
||||
_navigator->setRoll(packet.roll);
|
||||
_navigator->setPitch(packet.pitch);
|
||||
_navigator->setYaw(packet.yaw);
|
||||
_navigator->setRollRate(packet.rollspeed);
|
||||
_navigator->setPitchRate(packet.pitchspeed);
|
||||
_navigator->setYawRate(packet.yawspeed);
|
||||
//_hal->debug->printf_P(PSTR("received hil attitude packet\n"));
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_ACTION: {
|
||||
// decode
|
||||
mavlink_action_t packet;
|
||||
mavlink_msg_action_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target, packet.target_component))
|
||||
break;
|
||||
|
||||
// do action
|
||||
sendText(SEVERITY_LOW, PSTR("action received"));
|
||||
switch (packet.action) {
|
||||
|
||||
case MAV_ACTION_STORAGE_READ:
|
||||
AP_Var::load_all();
|
||||
break;
|
||||
|
||||
case MAV_ACTION_STORAGE_WRITE:
|
||||
AP_Var::save_all();
|
||||
break;
|
||||
|
||||
case MAV_ACTION_CALIBRATE_RC:
|
||||
case MAV_ACTION_CALIBRATE_GYRO:
|
||||
case MAV_ACTION_CALIBRATE_MAG:
|
||||
case MAV_ACTION_CALIBRATE_ACC:
|
||||
case MAV_ACTION_CALIBRATE_PRESSURE:
|
||||
case MAV_ACTION_REBOOT:
|
||||
case MAV_ACTION_REC_START:
|
||||
case MAV_ACTION_REC_PAUSE:
|
||||
case MAV_ACTION_REC_STOP:
|
||||
case MAV_ACTION_TAKEOFF:
|
||||
case MAV_ACTION_LAND:
|
||||
case MAV_ACTION_NAVIGATE:
|
||||
case MAV_ACTION_LOITER:
|
||||
case MAV_ACTION_MOTORS_START:
|
||||
case MAV_ACTION_CONFIRM_KILL:
|
||||
case MAV_ACTION_EMCY_KILL:
|
||||
case MAV_ACTION_MOTORS_STOP:
|
||||
case MAV_ACTION_SHUTDOWN:
|
||||
case MAV_ACTION_CONTINUE:
|
||||
case MAV_ACTION_SET_MANUAL:
|
||||
case MAV_ACTION_SET_AUTO:
|
||||
case MAV_ACTION_LAUNCH:
|
||||
case MAV_ACTION_RETURN:
|
||||
case MAV_ACTION_EMCY_LAND:
|
||||
case MAV_ACTION_HALT:
|
||||
sendText(SEVERITY_LOW, PSTR("action not implemented"));
|
||||
break;
|
||||
default:
|
||||
sendText(SEVERITY_LOW, PSTR("unknown action"));
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint request list"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_request_list_t packet;
|
||||
mavlink_msg_waypoint_request_list_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// Start sending waypoints
|
||||
mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
|
||||
_guide->getNumberOfCommands());
|
||||
|
||||
_cmdTimeLastSent = millis();
|
||||
_cmdTimeLastReceived = millis();
|
||||
_sendingCmds = true;
|
||||
_receivingCmds = false;
|
||||
_cmdDestSysId = msg->sysid;
|
||||
_cmdDestCompId = msg->compid;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint request"));
|
||||
|
||||
// Check if sending waypiont
|
||||
if (!_sendingCmds)
|
||||
break;
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_request_t packet;
|
||||
mavlink_msg_waypoint_request_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
|
||||
AP_MavlinkCommand cmd(packet.seq);
|
||||
|
||||
mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
|
||||
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
|
||||
wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
|
||||
wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
|
||||
wp.z);
|
||||
|
||||
// update last waypoint comm stamp
|
||||
_cmdTimeLastSent = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_ack_t packet;
|
||||
mavlink_msg_waypoint_ack_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// check for error
|
||||
//uint8_t type = packet.type; // ok (0), error(1)
|
||||
|
||||
// turn off waypoint send
|
||||
_sendingCmds = false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
sendText(SEVERITY_LOW, PSTR("param request list"));
|
||||
|
||||
// decode
|
||||
mavlink_param_request_list_t packet;
|
||||
mavlink_msg_param_request_list_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// Start sending parameters - next call to ::update will kick the first one out
|
||||
|
||||
_queuedParameter = AP_Var::first();
|
||||
_queuedParameterIndex = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint clear all"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_clear_all_t packet;
|
||||
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// clear all waypoints
|
||||
uint8_t type = 0; // ok (0), error(1)
|
||||
_guide->setNumberOfCommands(1);
|
||||
_guide->setCurrentIndex(0);
|
||||
|
||||
// send acknowledgement 3 times to makes sure it is received
|
||||
for (int i = 0; i < 3; i++)
|
||||
mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
|
||||
msg->compid, type);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint set current"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_set_current_t packet;
|
||||
mavlink_msg_waypoint_set_current_decode(msg, &packet);
|
||||
Serial.print("Packet Sequence:");
|
||||
Serial.println(packet.seq);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// set current waypoint
|
||||
Serial.print("Current Index:");
|
||||
Serial.println(_guide->getCurrentIndex());
|
||||
Serial.flush();
|
||||
_guide->setCurrentIndex(packet.seq);
|
||||
mavlink_msg_waypoint_current_send(_channel,
|
||||
_guide->getCurrentIndex());
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint count"));
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_count_t packet;
|
||||
mavlink_msg_waypoint_count_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// start waypoint receiving
|
||||
if (packet.count > _cmdMax) {
|
||||
packet.count = _cmdMax;
|
||||
}
|
||||
_cmdNumberRequested = packet.count;
|
||||
_cmdTimeLastReceived = millis();
|
||||
_receivingCmds = true;
|
||||
_sendingCmds = false;
|
||||
_cmdRequestIndex = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT: {
|
||||
sendText(SEVERITY_LOW, PSTR("waypoint"));
|
||||
|
||||
// Check if receiving waypiont
|
||||
if (!_receivingCmds) {
|
||||
//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
|
||||
break;
|
||||
}
|
||||
|
||||
// decode
|
||||
mavlink_waypoint_t packet;
|
||||
mavlink_msg_waypoint_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// check if this is the requested waypoint
|
||||
if (packet.seq != _cmdRequestIndex) {
|
||||
char warningMsg[50];
|
||||
sprintf(warningMsg,
|
||||
"waypoint request out of sequence: (packet) %d / %d (ap)",
|
||||
packet.seq, _cmdRequestIndex);
|
||||
sendText(SEVERITY_HIGH, warningMsg);
|
||||
break;
|
||||
}
|
||||
|
||||
_hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
|
||||
packet.x,
|
||||
packet.y,
|
||||
packet.z);
|
||||
|
||||
// store waypoint
|
||||
AP_MavlinkCommand command(packet);
|
||||
//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
|
||||
_cmdRequestIndex++;
|
||||
if (_cmdRequestIndex == _cmdNumberRequested) {
|
||||
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
|
||||
_receivingCmds = false;
|
||||
_guide->setNumberOfCommands(_cmdNumberRequested);
|
||||
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
|
||||
} else if (_cmdRequestIndex > _cmdNumberRequested) {
|
||||
_receivingCmds = false;
|
||||
}
|
||||
_cmdTimeLastReceived = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET: {
|
||||
sendText(SEVERITY_LOW, PSTR("param set"));
|
||||
AP_Var *vp;
|
||||
AP_Meta_class::Type_id var_type;
|
||||
|
||||
// decode
|
||||
mavlink_param_set_t packet;
|
||||
mavlink_msg_param_set_decode(msg, &packet);
|
||||
if (_checkTarget(packet.target_system, packet.target_component))
|
||||
break;
|
||||
|
||||
// set parameter
|
||||
|
||||
char key[_paramNameLengthMax + 1];
|
||||
strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
|
||||
key[_paramNameLengthMax] = 0;
|
||||
|
||||
// find the requested parameter
|
||||
vp = AP_Var::find(key);
|
||||
if ((NULL != vp) && // exists
|
||||
!isnan(packet.param_value) && // not nan
|
||||
!isinf(packet.param_value)) { // not inf
|
||||
|
||||
// add a small amount before casting parameter values
|
||||
// from float to integer to avoid truncating to the
|
||||
// next lower integer value.
|
||||
const float rounding_addition = 0.01;
|
||||
|
||||
// fetch the variable type ID
|
||||
var_type = vp->meta_type_id();
|
||||
|
||||
// handle variables with standard type IDs
|
||||
if (var_type == AP_Var::k_typeid_float) {
|
||||
((AP_Float *) vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_float16) {
|
||||
((AP_Float16 *) vp)->set_and_save(packet.param_value);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int32) {
|
||||
((AP_Int32 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int16) {
|
||||
((AP_Int16 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
|
||||
} else if (var_type == AP_Var::k_typeid_int8) {
|
||||
((AP_Int8 *) vp)->set_and_save(
|
||||
packet.param_value + rounding_addition);
|
||||
} else {
|
||||
// we don't support mavlink set on this parameter
|
||||
break;
|
||||
}
|
||||
|
||||
// Report back the new value if we accepted the change
|
||||
// we send the value we actually set, which could be
|
||||
// different from the value sent, in case someone sent
|
||||
// a fractional value to an integer type
|
||||
mavlink_msg_param_value_send(_channel, (int8_t *) key,
|
||||
vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
|
||||
}
|
||||
|
||||
break;
|
||||
} // end case
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t _countParameters() {
|
||||
// if we haven't cached the parameter count yet...
|
||||
if (0 == _parameterCount) {
|
||||
AP_Var *vp;
|
||||
|
||||
vp = AP_Var::first();
|
||||
do {
|
||||
// if a parameter responds to cast_to_float then we are going to be able to report it
|
||||
if (!isnan(vp->cast_to_float())) {
|
||||
_parameterCount++;
|
||||
}
|
||||
} while (NULL != (vp = vp->next()));
|
||||
}
|
||||
return _parameterCount;
|
||||
}
|
||||
|
||||
AP_Var * _findParameter(uint16_t index) {
|
||||
AP_Var *vp;
|
||||
|
||||
vp = AP_Var::first();
|
||||
while (NULL != vp) {
|
||||
|
||||
// if the parameter is reportable
|
||||
if (!(isnan(vp->cast_to_float()))) {
|
||||
// if we have counted down to the index we want
|
||||
if (0 == index) {
|
||||
// return the parameter
|
||||
return vp;
|
||||
}
|
||||
// count off this parameter, as it is reportable but not
|
||||
// the one we want
|
||||
index--;
|
||||
}
|
||||
// and move to the next parameter
|
||||
vp = vp->next();
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
AP_Var * _findParameter(uint16_t index);
|
||||
|
||||
// check the target
|
||||
uint8_t _checkTarget(uint8_t sysid, uint8_t compid) {
|
||||
/*
|
||||
char msg[50];
|
||||
sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid,
|
||||
mavlink_system.sysid, compid, mavlink_system.compid);
|
||||
sendText(SEVERITY_LOW, msg);
|
||||
*/
|
||||
if (sysid != mavlink_system.sysid) {
|
||||
//sendText(SEVERITY_LOW, PSTR("system id mismatch"));
|
||||
return 1;
|
||||
|
||||
} else if (compid != mavlink_system.compid) {
|
||||
//sendText(SEVERITY_LOW, PSTR("component id mismatch"));
|
||||
return 0; // XXX currently not receiving correct compid from gcs
|
||||
|
||||
} else {
|
||||
return 0; // no error
|
||||
}
|
||||
}
|
||||
uint8_t _checkTarget(uint8_t sysid, uint8_t compid);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -6,3 +6,28 @@
|
|||
*/
|
||||
|
||||
#include "AP_Controller.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "../AP_Common/include/menu.h"
|
||||
#include "AP_RcChannel.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
|
||||
AP_HardwareAbstractionLayer * hal) :
|
||||
_nav(nav), _guide(guide), _hal(hal) {
|
||||
setAllRadioChannelsToNeutral();
|
||||
}
|
||||
|
||||
void AP_Controller::setAllRadioChannelsToNeutral() {
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -19,37 +19,28 @@
|
|||
#ifndef AP_Controller_H
|
||||
#define AP_Controller_H
|
||||
|
||||
#include "AP_Navigator.h"
|
||||
#include "AP_Guide.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "../AP_Common/AP_Vector.h"
|
||||
#include "../AP_Common/AP_Var.h"
|
||||
#include <inttypes.h>
|
||||
#include "../GCS_MAVLink/GCS_MAVLink.h"
|
||||
#include <math.h>
|
||||
|
||||
class AP_Var_group;
|
||||
|
||||
namespace apo {
|
||||
|
||||
class AP_HardwareAbstractionLayer;
|
||||
class AP_Guide;
|
||||
class AP_Navigator;
|
||||
class Menu;
|
||||
|
||||
/// Controller class
|
||||
class AP_Controller {
|
||||
public:
|
||||
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
|
||||
AP_HardwareAbstractionLayer * hal) :
|
||||
_nav(nav), _guide(guide), _hal(hal) {
|
||||
}
|
||||
|
||||
AP_HardwareAbstractionLayer * hal);
|
||||
virtual void update(const float & dt) = 0;
|
||||
|
||||
virtual MAV_MODE getMode() = 0;
|
||||
|
||||
void setAllRadioChannelsToNeutral() {
|
||||
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
|
||||
_hal->rc[i]->setPosition(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
void setAllRadioChannelsManually() {
|
||||
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
|
||||
_hal->rc[i]->setUsingRadio();
|
||||
}
|
||||
}
|
||||
void setAllRadioChannelsToNeutral();
|
||||
void setAllRadioChannelsManually();
|
||||
|
||||
protected:
|
||||
AP_Navigator * _nav;
|
||||
|
@ -278,23 +269,30 @@ protected:
|
|||
class BlockPIDDfb: public AP_ControllerBlock {
|
||||
public:
|
||||
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
|
||||
float kD, float iMax, float yMax, const prog_char_t * dLabel = NULL) :
|
||||
float 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),
|
||||
_kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d")) {
|
||||
_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
|
||||
* derivative;
|
||||
* _blockLowPass.update(derivative,dt);
|
||||
return _blockSaturation.update(y);
|
||||
}
|
||||
protected:
|
||||
BlockP _blockP;
|
||||
BlockI _blockI;
|
||||
BlockSaturation _blockSaturation;
|
||||
BlockLowPass _blockLowPass;
|
||||
AP_Float _kD; /// derivative gain
|
||||
};
|
||||
|
||||
|
|
|
@ -6,3 +6,232 @@
|
|||
*/
|
||||
|
||||
#include "AP_Guide.h"
|
||||
#include "../FastSerial/FastSerial.h"
|
||||
#include "AP_Navigator.h"
|
||||
#include "constants.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "AP_CommLink.h"
|
||||
|
||||
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) {
|
||||
}
|
||||
|
||||
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) :
|
||||
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
|
||||
_rangeFinderLeft(), _rangeFinderRight(),
|
||||
_group(k_guide, PSTR("guide_")),
|
||||
_velocityCommand(&_group, 1, 1, PSTR("velCmd")),
|
||||
_crossTrackGain(&_group, 2, 1, PSTR("xt")),
|
||||
_crossTrackLim(&_group, 3, 90, 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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkGuide::update() {
|
||||
// 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;
|
||||
|
||||
//airSpeedCommand = 0;
|
||||
//groundSpeedCommand = 0;
|
||||
// _headingCommand -= 45 * deg2Rad;
|
||||
// _hal->debug->print("Obstacle Distance (m): ");
|
||||
// _hal->debug->println(frontDistance);
|
||||
// _hal->debug->print("Obstacle avoidance Heading Command: ");
|
||||
// _hal->debug->println(headingCommand);
|
||||
// _hal->debug->printf_P(
|
||||
// PSTR("Front Distance, %f\n"),
|
||||
// frontDistance);
|
||||
}
|
||||
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
|
||||
_airSpeedCommand = 0;
|
||||
_groundSpeedCommand = 0;
|
||||
|
||||
}
|
||||
|
||||
if (_rangeFinderLeft && _rangeFinderLeft->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;
|
||||
}
|
||||
|
||||
_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()) {
|
||||
|
||||
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;
|
||||
|
||||
//_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);
|
||||
}
|
||||
|
||||
_groundSpeedCommand = _velocityCommand;
|
||||
|
||||
// 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());
|
||||
|
||||
// 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:
|
||||
// case MAV_CMD_CONDITION_LAST:
|
||||
// case MAV_CMD_CONDITION_YAW:
|
||||
// case MAV_CMD_DO_CHANGE_SPEED:
|
||||
// case MAV_CMD_DO_CONTROL_VIDEO:
|
||||
// case MAV_CMD_DO_JUMP:
|
||||
// case MAV_CMD_DO_LAST:
|
||||
// case MAV_CMD_DO_LAST:
|
||||
// case MAV_CMD_DO_REPEAT_RELAY:
|
||||
// case MAV_CMD_DO_REPEAT_SERVO:
|
||||
// case MAV_CMD_DO_SET_HOME:
|
||||
// case MAV_CMD_DO_SET_MODE:
|
||||
// case MAV_CMD_DO_SET_PARAMETER:
|
||||
// case MAV_CMD_DO_SET_RELAY:
|
||||
// case MAV_CMD_DO_SET_SERVO:
|
||||
// case MAV_CMD_PREFLIGHT_CALIBRATION:
|
||||
// case MAV_CMD_PREFLIGHT_STORAGE:
|
||||
// case MAV_CMD_NAV_LAND:
|
||||
// case MAV_CMD_NAV_LAST:
|
||||
// case MAV_CMD_NAV_LOITER_TIME:
|
||||
// case MAV_CMD_NAV_LOITER_TURNS:
|
||||
// case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
// case MAV_CMD_NAV_ORIENTATION_TARGET:
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace apo
|
||||
|
||||
// vim:ts=4:sw=4:expandtab
|
||||
|
|
|
@ -19,18 +19,16 @@
|
|||
#ifndef AP_Guide_H
|
||||
#define AP_Guide_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "../GCS_MAVLink/GCS_MAVLink.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "AP_Navigator.h"
|
||||
#include "../AP_Common/AP_Common.h"
|
||||
#include "../AP_Common/AP_Vector.h"
|
||||
#include "AP_MavlinkCommand.h"
|
||||
#include "constants.h"
|
||||
|
||||
//#include "AP_CommLink.h"
|
||||
#include "../AP_RangeFinder/AP_RangeFinder.h"
|
||||
|
||||
namespace apo {
|
||||
|
||||
class AP_Navigator;
|
||||
class AP_HardwareAbstractionLayer;
|
||||
|
||||
/// Guide class
|
||||
class AP_Guide {
|
||||
public:
|
||||
|
@ -39,15 +37,7 @@ 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) :
|
||||
_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) {
|
||||
}
|
||||
AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal);
|
||||
|
||||
virtual void update() = 0;
|
||||
|
||||
|
@ -60,12 +50,7 @@ public:
|
|||
return _cmdIndex;
|
||||
}
|
||||
|
||||
void 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 setCurrentIndex(uint8_t val);
|
||||
|
||||
uint8_t getNumberOfCommands() {
|
||||
return _numberOfCommands;
|
||||
|
@ -140,205 +125,11 @@ protected:
|
|||
class MavlinkGuide: public AP_Guide {
|
||||
public:
|
||||
MavlinkGuide(AP_Navigator * navigator,
|
||||
AP_HardwareAbstractionLayer * hal) :
|
||||
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
|
||||
_rangeFinderLeft(), _rangeFinderRight(),
|
||||
_group(k_guide, PSTR("guide_")),
|
||||
_velocityCommand(&_group, 1, 1, PSTR("velCmd")),
|
||||
_crossTrackGain(&_group, 2, 1, PSTR("xt")),
|
||||
_crossTrackLim(&_group, 3, 90, PSTR("xtLim")) {
|
||||
AP_HardwareAbstractionLayer * hal);
|
||||
virtual void update();
|
||||
void nextCommand();
|
||||
void handleCommand();
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
virtual void update() {
|
||||
// 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;
|
||||
|
||||
//airSpeedCommand = 0;
|
||||
//groundSpeedCommand = 0;
|
||||
// _headingCommand -= 45 * deg2Rad;
|
||||
// _hal->debug->print("Obstacle Distance (m): ");
|
||||
// _hal->debug->println(frontDistance);
|
||||
// _hal->debug->print("Obstacle avoidance Heading Command: ");
|
||||
// _hal->debug->println(headingCommand);
|
||||
// _hal->debug->printf_P(
|
||||
// PSTR("Front Distance, %f\n"),
|
||||
// frontDistance);
|
||||
}
|
||||
if (_rangeFinderBack && _rangeFinderBack->distance < 5) {
|
||||
_airSpeedCommand = 0;
|
||||
_groundSpeedCommand = 0;
|
||||
|
||||
}
|
||||
|
||||
if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) {
|
||||
_airSpeedCommand = 0;
|
||||
_groundSpeedCommand = 0;
|
||||
}
|
||||
|
||||
if (_rangeFinderRight && _rangeFinderRight->distance < 5) {
|
||||
_airSpeedCommand = 0;
|
||||
_groundSpeedCommand = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void 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;
|
||||
}
|
||||
|
||||
_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 handleCommand() {
|
||||
|
||||
// TODO handle more commands
|
||||
switch (_command.getCommand()) {
|
||||
|
||||
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;
|
||||
|
||||
//_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);
|
||||
}
|
||||
|
||||
_groundSpeedCommand = _velocityCommand;
|
||||
|
||||
// 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());
|
||||
|
||||
// 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:
|
||||
// case MAV_CMD_CONDITION_LAST:
|
||||
// case MAV_CMD_CONDITION_YAW:
|
||||
// case MAV_CMD_DO_CHANGE_SPEED:
|
||||
// case MAV_CMD_DO_CONTROL_VIDEO:
|
||||
// case MAV_CMD_DO_JUMP:
|
||||
// case MAV_CMD_DO_LAST:
|
||||
// case MAV_CMD_DO_LAST:
|
||||
// case MAV_CMD_DO_REPEAT_RELAY:
|
||||
// case MAV_CMD_DO_REPEAT_SERVO:
|
||||
// case MAV_CMD_DO_SET_HOME:
|
||||
// case MAV_CMD_DO_SET_MODE:
|
||||
// case MAV_CMD_DO_SET_PARAMETER:
|
||||
// case MAV_CMD_DO_SET_RELAY:
|
||||
// case MAV_CMD_DO_SET_SERVO:
|
||||
// case MAV_CMD_PREFLIGHT_CALIBRATION:
|
||||
// case MAV_CMD_PREFLIGHT_STORAGE:
|
||||
// case MAV_CMD_NAV_LAND:
|
||||
// case MAV_CMD_NAV_LAST:
|
||||
// case MAV_CMD_NAV_LOITER_TIME:
|
||||
// case MAV_CMD_NAV_LOITER_TURNS:
|
||||
// case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
// case MAV_CMD_NAV_ORIENTATION_TARGET:
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
private:
|
||||
RangeFinder * _rangeFinderFront;
|
||||
RangeFinder * _rangeFinderBack;
|
||||
|
|
|
@ -8,20 +8,9 @@
|
|||
#ifndef AP_HARDWAREABSTRACTIONLAYER_H_
|
||||
#define AP_HARDWAREABSTRACTIONLAYER_H_
|
||||
|
||||
#include "../AP_Common/AP_Common.h"
|
||||
#include "../FastSerial/FastSerial.h"
|
||||
#include "../AP_Common/AP_Vector.h"
|
||||
#include "../GCS_MAVLink/GCS_MAVLink.h"
|
||||
|
||||
#include "../AP_ADC/AP_ADC.h"
|
||||
#include "../AP_IMU/AP_IMU.h"
|
||||
#include "../AP_GPS/GPS.h"
|
||||
#include "../APM_BMP085/APM_BMP085.h"
|
||||
#include "../AP_Compass/AP_Compass.h"
|
||||
#include "AP_RcChannel.h"
|
||||
#include "../AP_RangeFinder/AP_RangeFinder.h"
|
||||
#include "../GCS_MAVLink/GCS_MAVLink.h"
|
||||
|
||||
class AP_ADC;
|
||||
class IMU;
|
||||
class GPS;
|
||||
|
@ -29,11 +18,13 @@ class APM_BMP085_Class;
|
|||
class Compass;
|
||||
class BetterStream;
|
||||
class RangeFinder;
|
||||
class FastSerial;
|
||||
|
||||
namespace apo {
|
||||
|
||||
class AP_RcChannel;
|
||||
class AP_CommLink;
|
||||
class AP_BatteryMonitor;
|
||||
|
||||
// enumerations
|
||||
enum halMode_t {
|
||||
|
@ -50,9 +41,12 @@ 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(), rc(), gcs(),
|
||||
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) {
|
||||
|
@ -98,6 +92,7 @@ public:
|
|||
Compass * compass;
|
||||
Vector<RangeFinder *> rangeFinders;
|
||||
IMU * imu;
|
||||
AP_BatteryMonitor * batteryMonitor;
|
||||
|
||||
/**
|
||||
* Radio Channels
|
||||
|
|
|
@ -6,3 +6,189 @@
|
|||
*/
|
||||
|
||||
#include "AP_Navigator.h"
|
||||
#include "../FastSerial/FastSerial.h"
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "../AP_DCM/AP_DCM.h"
|
||||
#include "../AP_Math/AP_Math.h"
|
||||
#include "../AP_Compass/AP_Compass.h"
|
||||
#include "AP_MavlinkCommand.h"
|
||||
#include "AP_Var_keys.h"
|
||||
#include "../AP_RangeFinder/AP_RangeFinder.h"
|
||||
#include "../AP_IMU/AP_IMU.h"
|
||||
#include "../APM_BMP085/APM_BMP085.h"
|
||||
|
||||
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) {
|
||||
}
|
||||
void AP_Navigator::calibrate() {
|
||||
}
|
||||
float AP_Navigator::getPD() const {
|
||||
return AP_MavlinkCommand::home.getPD(getAlt_intM());
|
||||
}
|
||||
|
||||
float AP_Navigator::getPE() const {
|
||||
return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
|
||||
}
|
||||
|
||||
float AP_Navigator::getPN() const {
|
||||
return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
|
||||
}
|
||||
|
||||
void AP_Navigator::setPD(float _pD) {
|
||||
setAlt(AP_MavlinkCommand::home.getAlt(_pD));
|
||||
}
|
||||
|
||||
void AP_Navigator::setPE(float _pE) {
|
||||
setLat(AP_MavlinkCommand::home.getLat(_pE));
|
||||
}
|
||||
|
||||
void AP_Navigator::setPN(float _pN) {
|
||||
setLon(AP_MavlinkCommand::home.getLon(_pN));
|
||||
}
|
||||
|
||||
DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) :
|
||||
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 (_hal->getMode() == MODE_LIVE) {
|
||||
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->compass) {
|
||||
_dcm->set_compass(_hal->compass);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void DcmNavigator::calibrate() {
|
||||
|
||||
AP_Navigator::calibrate();
|
||||
|
||||
// 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;
|
||||
|
||||
setTimeStamp(micros()); // if running in live mode, record new time stamp
|
||||
|
||||
|
||||
//_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) {
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// dcm class for attitude
|
||||
if (_dcm) {
|
||||
_dcm->update_DCM();
|
||||
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);
|
||||
*/
|
||||
}
|
||||
}
|
||||
void DcmNavigator::updateSlow(float dt) {
|
||||
if (_hal->getMode() != MODE_LIVE)
|
||||
return;
|
||||
|
||||
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->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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace apo
|
||||
|
|
|
@ -19,54 +19,30 @@
|
|||
#ifndef AP_Navigator_H
|
||||
#define AP_Navigator_H
|
||||
|
||||
#include "AP_HardwareAbstractionLayer.h"
|
||||
#include "../AP_DCM/AP_DCM.h"
|
||||
#include "../AP_Math/AP_Math.h"
|
||||
#include "../AP_Compass/AP_Compass.h"
|
||||
#include "AP_MavlinkCommand.h"
|
||||
#include "constants.h"
|
||||
#include "AP_Var_keys.h"
|
||||
#include "../AP_RangeFinder/AP_RangeFinder.h"
|
||||
#include "../AP_IMU/AP_IMU.h"
|
||||
#include <inttypes.h>
|
||||
|
||||
class RangeFinder;
|
||||
class IMU;
|
||||
class AP_DCM;
|
||||
|
||||
namespace apo {
|
||||
|
||||
class AP_HardwareAbstractionLayer;
|
||||
|
||||
/// Navigator class
|
||||
class AP_Navigator {
|
||||
public:
|
||||
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) {
|
||||
}
|
||||
virtual void calibrate() {
|
||||
}
|
||||
AP_Navigator(AP_HardwareAbstractionLayer * hal);
|
||||
virtual void calibrate();
|
||||
virtual void updateFast(float dt) = 0;
|
||||
virtual void updateSlow(float dt) = 0;
|
||||
float getPD() const {
|
||||
return AP_MavlinkCommand::home.getPD(getAlt_intM());
|
||||
}
|
||||
|
||||
float getPE() const {
|
||||
return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt());
|
||||
}
|
||||
|
||||
float getPN() const {
|
||||
return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt());
|
||||
}
|
||||
|
||||
void setPD(float _pD) {
|
||||
setAlt(AP_MavlinkCommand::home.getAlt(_pD));
|
||||
}
|
||||
|
||||
void setPE(float _pE) {
|
||||
setLat(AP_MavlinkCommand::home.getLat(_pE));
|
||||
}
|
||||
|
||||
void setPN(float _pN) {
|
||||
setLon(AP_MavlinkCommand::home.getLon(_pN));
|
||||
}
|
||||
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;
|
||||
|
@ -241,146 +217,11 @@ private:
|
|||
uint16_t _imuOffsetAddress;
|
||||
|
||||
public:
|
||||
DcmNavigator(AP_HardwareAbstractionLayer * hal) :
|
||||
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 (_hal->getMode() == MODE_LIVE) {
|
||||
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->compass) {
|
||||
_dcm->set_compass(_hal->compass);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
virtual void calibrate() {
|
||||
|
||||
AP_Navigator::calibrate();
|
||||
|
||||
// TODO: handle cold/warm restart
|
||||
if (_hal->imu) {
|
||||
_hal->imu->init(IMU::COLD_START,delay);
|
||||
}
|
||||
}
|
||||
virtual void updateFast(float dt) {
|
||||
|
||||
if (_hal->getMode() != MODE_LIVE)
|
||||
return;
|
||||
|
||||
setTimeStamp(micros()); // if running in live mode, record new time stamp
|
||||
|
||||
|
||||
//_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) {
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// dcm class for attitude
|
||||
if (_dcm) {
|
||||
_dcm->update_DCM();
|
||||
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);
|
||||
*/
|
||||
}
|
||||
}
|
||||
virtual void updateSlow(float dt) {
|
||||
if (_hal->getMode() != MODE_LIVE)
|
||||
return;
|
||||
|
||||
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->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 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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
DcmNavigator(AP_HardwareAbstractionLayer * hal);
|
||||
virtual void calibrate();
|
||||
virtual void updateFast(float dt);
|
||||
virtual void updateSlow(float dt);
|
||||
void updateGpsLight(void);
|
||||
};
|
||||
|
||||
} // namespace apo
|
||||
|
|
|
@ -30,7 +30,7 @@ public:
|
|||
Vector<Loop *> & subLoops() {
|
||||
return _subLoops;
|
||||
}
|
||||
uint32_t frequency() {
|
||||
float frequency() {
|
||||
return 1.0e6/_period;
|
||||
}
|
||||
void frequency(float _frequency) {
|
||||
|
@ -45,7 +45,7 @@ public:
|
|||
uint8_t load() {
|
||||
return _load;
|
||||
}
|
||||
private:
|
||||
protected:
|
||||
void (*_fptr)(void *);
|
||||
void * _data;
|
||||
uint32_t _period;
|
||||
|
|
Loading…
Reference in New Issue