diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index e8f3675817..847f5617fd 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -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 diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 4c733e5013..32f402035d 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -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; diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 36eaefc552..cbdd2c6688 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -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() { diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 411edc0c3c..61a52e0ce7 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -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 diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h index 97c53cd470..e6684fe07f 100644 --- a/libraries/APO/AP_Autopilot.h +++ b/libraries/APO/AP_Autopilot.h @@ -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 diff --git a/libraries/APO/AP_BatteryMonitor.cpp b/libraries/APO/AP_BatteryMonitor.cpp new file mode 100644 index 0000000000..e5550d2798 --- /dev/null +++ b/libraries/APO/AP_BatteryMonitor.cpp @@ -0,0 +1,10 @@ +/* + * AP_BatteryMonitor.cpp + * + */ + +#include "AP_BatteryMonitor.h" + +namespace apo { + +} // apo diff --git a/libraries/APO/AP_BatteryMonitor.h b/libraries/APO/AP_BatteryMonitor.h new file mode 100644 index 0000000000..7f82db349d --- /dev/null +++ b/libraries/APO/AP_BatteryMonitor.h @@ -0,0 +1,51 @@ +/* + * AP_BatteryMonitor.h + * + */ + +#ifndef AP_BATTERYMONITOR_H_ +#define AP_BATTERYMONITOR_H_ + +#include +#include + +namespace apo { + +class AP_BatteryMonitor { +public: + AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) : + _pin(pin), _voltageDivRatio(voltageDivRatio), + _minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) { + } + + void update() { + // low pass filter on voltage + _voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1; + } + + /** + * Accessors + */ + float getVoltage() { + return _voltage; + } + + uint8_t getPercentage() { + float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt); + if (norm < 0) norm = 0; + else if (norm > 1) norm = 1; + return 100*norm; + } + +private: + + uint8_t _pin; + float _voltageDivRatio; + float _voltage; + float _minVolt; + float _maxVolt; +}; + +} // namespace apo + +#endif /* AP_BATTERYMONITOR_H_ */ diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 92b9d0fdde..cad558ccbd 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -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 diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h index b40641b143..be473cde9a 100644 --- a/libraries/APO/AP_CommLink.h +++ b/libraries/APO/AP_CommLink.h @@ -19,9 +19,12 @@ #ifndef AP_CommLink_H #define AP_CommLink_H -#include "AP_HardwareAbstractionLayer.h" +#include +#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); }; diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index ebbd5ed524..443fe2afbb 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -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(); + } +} + +} diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index d7dac0c60c..4cfc57609d 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -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 +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include + +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 }; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 488b746610..def30d834c 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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 diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 8f5b61929b..0944620247 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -19,18 +19,16 @@ #ifndef AP_Guide_H #define AP_Guide_H +#include #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; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 45e7daaf75..422bb55d36 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -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 rangeFinders; IMU * imu; + AP_BatteryMonitor * batteryMonitor; /** * Radio Channels diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index ff33e8ed2c..46e74d4d87 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -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 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 diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 8330112541..9be22e6caf 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -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 + +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 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 diff --git a/libraries/AP_Common/AP_Loop.h b/libraries/AP_Common/AP_Loop.h index ef2ebe25c6..313ce43e79 100644 --- a/libraries/AP_Common/AP_Loop.h +++ b/libraries/AP_Common/AP_Loop.h @@ -30,7 +30,7 @@ public: Vector & 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;