From f6fe6fde336b8de8ddf49601d92d232b4117b265 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 6 Oct 2011 19:17:49 -0400 Subject: [PATCH] Updated APO, HIL working well, live mode ready to test --- ArduRover/ArduRover.pde | 2 +- ArduRover/CarStampede.h | 2 +- ArduRover/TankGeneric.h | 18 ++-- apo/ControllerPlane.h | 2 +- apo/PlaneEasystar.h | 4 +- libraries/APO/APO_DefaultSetup.h | 56 ++++++----- libraries/APO/AP_Autopilot.cpp | 48 +++++----- libraries/APO/AP_CommLink.h | 3 +- libraries/APO/AP_Guide.h | 144 ++++++++++++++-------------- libraries/APO/AP_MavlinkCommand.cpp | 22 ++++- libraries/APO/AP_Navigator.h | 8 +- 11 files changed, 164 insertions(+), 145 deletions(-) diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde index f6a1f17ac3..cc1a79aa0c 100644 --- a/ArduRover/ArduRover.pde +++ b/ArduRover/ArduRover.pde @@ -15,7 +15,7 @@ #include // Vehicle Configuration -#include "CarStampede.h" +#include "TankGeneric.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 3ed8a0b2b1..07a4fe20e4 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -12,7 +12,7 @@ // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_CAR; static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3; // algorithm selection diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h index 30d7118582..9af4f9b1d4 100644 --- a/ArduRover/TankGeneric.h +++ b/ArduRover/TankGeneric.h @@ -11,7 +11,7 @@ // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_TANK; static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3; // algorithm selection @@ -32,15 +32,15 @@ static const uint8_t heartBeatTimeout = 3; // optional sensors static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static bool baroEnabled = false; +static bool compassEnabled = false; -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +static bool rangeFinderFrontEnabled = false; +static bool rangeFinderBackEnabled = false; +static bool rangeFinderLeftEnabled = false; +static bool rangeFinderRightEnabled = false; +static bool rangeFinderUpEnabled = false; +static bool rangeFinderDownEnabled = false; // loop rates static const float loop0Rate = 150; diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 2120ead7fb..408cb04491 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -83,7 +83,7 @@ public: pidAltThrLim, pidAltThrDFCut), requireRadio(false) { - _hal->debug->println_P(PSTR("initializing car controller")); + _hal->debug->println_P(PSTR("initializing plane controller")); _hal->rc.push_back( new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h index 6acf27b403..812c4ad449 100644 --- a/apo/PlaneEasystar.h +++ b/apo/PlaneEasystar.h @@ -55,7 +55,7 @@ static const float loop4Rate = 0.1; static const float rdrAilMix = 1.0; // since there are no ailerons // bank error to roll servo -static const float pidBnkRllP = -0.5; +static const float pidBnkRllP = -1; static const float pidBnkRllI = 0.0; static const float pidBnkRllD = 0.0; static const float pidBnkRllAwu = 0.0; @@ -79,7 +79,7 @@ static const float pidSpdPitLim = 1.0; static const float pidSpdPitDFCut = 0.0; // yaw rate error to yaw servo -static const float pidYwrYawP = -0.2; +static const float pidYwrYawP = -0.1; static const float pidYwrYawI = 0.0; static const float pidYwrYawD = 0.0; static const float pidYwrYawAwu = 0.0; diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index c718f3ac65..a74b58535e 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -18,40 +18,35 @@ void setup() { AP_Var::load_all(); + // Declare all parts of the system + AP_Navigator * navigator = NULL; + AP_Guide * guide = NULL; + AP_Controller * controller = NULL; + AP_HardwareAbstractionLayer * hal = NULL; /* * Communications */ Serial.begin(DEBUG_BAUD, 128, 128); // debug - if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs - else Serial3.begin(TELEM_BAUD, 128, 128); // gcs - + // hardware abstraction layer - AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + hal = new AP_HardwareAbstractionLayer( halMode, board, vehicle, heartBeatTimeout); // debug serial hal->debug = &Serial; hal->debug->println_P(PSTR("initializing debug line")); - /* - * Initialize Comm Channels - */ - hal->debug->println_P(PSTR("initializing comm channels")); - if (hal->getMode() == MODE_LIVE) { - Serial1.begin(GPS_BAUD, 128, 16); // gps - } else { // hil - Serial1.begin(HIL_BAUD, 128, 128); - } - /* * Sensor initialization */ if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); hal->adc = new ADC_CLASS; hal->adc->Init(); if (gpsEnabled) { + Serial1.begin(GPS_BAUD, 128, 16); // gps hal->debug->println_P(PSTR("initializing gps")); AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); hal->gps = &gpsDriver; @@ -128,28 +123,45 @@ void setup() { hal->rangeFinders.push_back(rangeFinder); } - } + } else /* * Select guidance, navigation, control algorithms */ - AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); - AP_Guide * guide = new GUIDE_CLASS(navigator, hal); - AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + navigator = new NAVIGATOR_CLASS(hal); + guide = new GUIDE_CLASS(navigator, hal); + controller = new CONTROLLER_CLASS(navigator, guide, hal); /* * CommLinks */ - if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); - else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); - - hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + if (board==BOARD_ARDUPILOTMEGA_2) + { + Serial2.begin(TELEM_BAUD, 128, 128); // gcs + hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + } + else + { + Serial3.begin(TELEM_BAUD, 128, 128); // gcs + hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + } + + /* + * Hardware in the Loop + */ + if (hal->getMode() == MODE_HIL_CNTL) { + Serial.println("HIL line setting up"); + Serial1.begin(HIL_BAUD, 128, 128); + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + } + /* * Start the autopilot */ hal->debug->printf_P(PSTR("initializing arduplane\n")); hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, loop0Rate, loop1Rate, loop2Rate, loop3Rate); } diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index f7ed3dca1f..55251c720d 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -29,24 +29,19 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, hal->setState(MAV_STATE_CALIBRATING); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - navigator->calibrate(); - // start clock - //uint32_t timeStart = millis(); - //uint16_t gpsWaitTime = 5000; // 5 second wait for gps + if (navigator) navigator->calibrate(); /* * Look for valid initial state */ - while (1) { + while (_navigator) { // letc gcs known we are alive hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - delay(1000); if (hal->getMode() == MODE_LIVE) { _navigator->updateSlow(0); - if (_hal->gps) { + if (hal->gps) { if (hal->gps->fix) { break; } else { @@ -58,7 +53,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, break; } } else if (hal->getMode() == MODE_HIL_CNTL) { // hil - _hal->hil->receive(); + hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->hil->receive(); Serial.println("HIL Receive Called"); if (_navigator->getTimeStamp() != 0) { // give hil a chance to send some packets @@ -71,20 +67,24 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, break; } hal->debug->println_P(PSTR("waiting for hil packet")); + delay(1000); } } - + AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); AP_MavlinkCommand::home.setLat(_navigator->getLat()); AP_MavlinkCommand::home.setLon(_navigator->getLon()); + AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT); AP_MavlinkCommand::home.save(); - _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg\n"), + _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), AP_MavlinkCommand::home.getLat()*rad2Deg, - AP_MavlinkCommand::home.getLon()*rad2Deg); + AP_MavlinkCommand::home.getLon()*rad2Deg, + AP_MavlinkCommand::home.getCommand()); AP_MavlinkCommand::home.load(); - _hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"), + _hal->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), AP_MavlinkCommand::home.getLat()*rad2Deg, - AP_MavlinkCommand::home.getLon()*rad2Deg); + AP_MavlinkCommand::home.getLon()*rad2Deg, + AP_MavlinkCommand::home.getCommand()); /* * Attach loops @@ -135,16 +135,7 @@ void AP_Autopilot::callback1(void * data) { apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); } - /* - * update guidance laws - */ - if (apo->getGuide()) - { - //apo->getHal()->debug->println_P(PSTR("updating guide")); - apo->getGuide()->update(); - } - - /* + /* * update control laws */ if (apo->getController()) { @@ -161,6 +152,15 @@ void AP_Autopilot::callback1(void * data) { void AP_Autopilot::callback2(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; //apo->getHal()->debug->println_P(PSTR("callback 2")); + + /* + * update guidance laws + */ + if (apo->getGuide()) + { + //apo->getHal()->debug->println_P(PSTR("updating guide")); + apo->getGuide()->update(); + } /* * send telemetry diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h index 08d20be57f..b40641b143 100644 --- a/libraries/APO/AP_CommLink.h +++ b/libraries/APO/AP_CommLink.h @@ -359,7 +359,7 @@ private: uint32_t timeStamp = micros(); switch (msg->msgid) { - //_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + _hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; @@ -497,7 +497,6 @@ private: _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); AP_MavlinkCommand cmd(packet.seq); - cmd.load(); mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index d546bacb4e..8f5b61929b 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -40,7 +40,8 @@ public: * @param navigator This is the navigator pointer. */ AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : - _navigator(navigator), _hal(hal), _command(0), _previousCommand(0), + _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home), + _previousCommand(AP_MavlinkCommand::home), _headingCommand(0), _airSpeedCommand(0), _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), @@ -144,8 +145,8 @@ public: _rangeFinderLeft(), _rangeFinderRight(), _group(k_guide, PSTR("guide_")), _velocityCommand(&_group, 1, 1, PSTR("velCmd")), - _crossTrackGain(&_group, 2, 2, PSTR("xt")), - _crossTrackLim(&_group, 3, 10, PSTR("xtLim")) { + _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]; @@ -168,67 +169,8 @@ public: } virtual void update() { -// _hal->debug->printf_P( -// PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), -// getNumberOfCommands(), -// getCurrentIndex(), -// getPreviousIndex()); - - // 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())); - } else { - _mode = MAV_NAV_WAYPOINT; - _altitudeCommand = _command.getAlt(); - // TODO wrong behavior if 0 selected as waypoint, says previous 0 - 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; - 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("radius reached"); - nextCommand(); - } -// _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; - - // TODO : 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()); - // process mavlink commands - //handleCommand(); + handleCommand(); // obstacle avoidance overrides // stop if your going to drive into something in front of you @@ -237,6 +179,7 @@ public: 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; @@ -291,18 +234,74 @@ public: _previousCommand = AP_MavlinkCommand(getPreviousIndex()); } - void handleCommand(AP_MavlinkCommand command, - AP_MavlinkCommand previousCommand) { + void handleCommand() { + // TODO handle more commands - switch (command.getCommand()) { + switch (_command.getCommand()) { + case MAV_CMD_NAV_WAYPOINT: { - // if within radius, increment - float d = previousCommand.distanceTo(_navigator->getLat_degInt(), - _navigator->getLon_degInt()); - if (d < command.getRadius()) { - nextCommand(); - Serial.println("radius reached"); + + // 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: @@ -354,4 +353,5 @@ private: } // namespace apo #endif // AP_Guide_H + // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index acebc1f208..987e2c0dbe 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -24,6 +24,19 @@ AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) : Serial.print("key: "); Serial.println(k_commands + index); Serial.println("++"); } + + // default values for structure + _data.get().command = MAV_CMD_NAV_WAYPOINT; + _data.get().autocontinue = true; + _data.get().frame = MAV_FRAME_GLOBAL; + _data.get().param1 = 0; + _data.get().param2 = 10; // radius of 10 meters + _data.get().param3 = 0; + _data.get().param4 = 0; + _data.get().x = 0; + _data.get().y = 0; + _data.get().z = 1000; + // This is a failsafe measure to stop trying to load a command if it can't load if (doLoad && !load()) { Serial.println("load failed, reverting to home waypoint"); @@ -153,11 +166,10 @@ float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) cons //calculates cross track of a current location float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const { - float d = previous.distanceTo(lat_degInt, lon_degInt); - float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); - float bNext = previous.bearingTo(*this); - return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; - return 0; + float d = previous.distanceTo(lat_degInt, lon_degInt); + float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); + float bNext = previous.bearingTo(*this); + return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; } // calculates along track distance of a current location diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 9b87561946..8170aa826a 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -273,13 +273,9 @@ public: AP_Navigator::calibrate(); - // TODO: handle cold restart + // TODO: handle cold/warm restart if (_hal->imu) { - /* - * Gyro has built in warm up cycle and should - * run first */ - _hal->imu->init_gyro(NULL); - _hal->imu->init_accel(NULL); + _hal->imu->init(IMU::COLD_START,delay); } } virtual void updateFast(float dt) {