Updated APO, HIL working well, live mode ready to test

This commit is contained in:
James Goppert 2011-10-06 19:17:49 -04:00
parent f77345de8f
commit f6fe6fde33
11 changed files with 164 additions and 145 deletions

View File

@ -15,7 +15,7 @@
#include <APO.h> #include <APO.h>
// Vehicle Configuration // Vehicle Configuration
#include "CarStampede.h" #include "TankGeneric.h"
// ArduPilotOne Default Setup // ArduPilotOne Default Setup
#include "APO_DefaultSetup.h" #include "APO_DefaultSetup.h"

View File

@ -12,7 +12,7 @@
// vehicle options // vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_CAR; static const apo::vehicle_t vehicle = apo::VEHICLE_CAR;
static const apo::halMode_t halMode = apo::MODE_LIVE; 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; static const uint8_t heartBeatTimeout = 3;
// algorithm selection // algorithm selection

View File

@ -11,7 +11,7 @@
// vehicle options // vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_TANK; static const apo::vehicle_t vehicle = apo::VEHICLE_TANK;
static const apo::halMode_t halMode = apo::MODE_LIVE; 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; static const uint8_t heartBeatTimeout = 3;
// algorithm selection // algorithm selection
@ -32,15 +32,15 @@ static const uint8_t heartBeatTimeout = 3;
// optional sensors // optional sensors
static bool gpsEnabled = false; static bool gpsEnabled = false;
static bool baroEnabled = true; static bool baroEnabled = false;
static bool compassEnabled = true; static bool compassEnabled = false;
static bool rangeFinderFrontEnabled = true; static bool rangeFinderFrontEnabled = false;
static bool rangeFinderBackEnabled = true; static bool rangeFinderBackEnabled = false;
static bool rangeFinderLeftEnabled = true; static bool rangeFinderLeftEnabled = false;
static bool rangeFinderRightEnabled = true; static bool rangeFinderRightEnabled = false;
static bool rangeFinderUpEnabled = true; static bool rangeFinderUpEnabled = false;
static bool rangeFinderDownEnabled = true; static bool rangeFinderDownEnabled = false;
// loop rates // loop rates
static const float loop0Rate = 150; static const float loop0Rate = 150;

View File

@ -83,7 +83,7 @@ public:
pidAltThrLim, pidAltThrDFCut), pidAltThrLim, pidAltThrDFCut),
requireRadio(false) { requireRadio(false) {
_hal->debug->println_P(PSTR("initializing car controller")); _hal->debug->println_P(PSTR("initializing plane controller"));
_hal->rc.push_back( _hal->rc.push_back(
new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100,

View File

@ -55,7 +55,7 @@ static const float loop4Rate = 0.1;
static const float rdrAilMix = 1.0; // since there are no ailerons static const float rdrAilMix = 1.0; // since there are no ailerons
// bank error to roll servo // 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 pidBnkRllI = 0.0;
static const float pidBnkRllD = 0.0; static const float pidBnkRllD = 0.0;
static const float pidBnkRllAwu = 0.0; static const float pidBnkRllAwu = 0.0;
@ -79,7 +79,7 @@ static const float pidSpdPitLim = 1.0;
static const float pidSpdPitDFCut = 0.0; static const float pidSpdPitDFCut = 0.0;
// yaw rate error to yaw servo // 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 pidYwrYawI = 0.0;
static const float pidYwrYawD = 0.0; static const float pidYwrYawD = 0.0;
static const float pidYwrYawAwu = 0.0; static const float pidYwrYawAwu = 0.0;

View File

@ -18,40 +18,35 @@ void setup() {
AP_Var::load_all(); 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 * Communications
*/ */
Serial.begin(DEBUG_BAUD, 128, 128); // debug 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 // hardware abstraction layer
AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( hal = new AP_HardwareAbstractionLayer(
halMode, board, vehicle, heartBeatTimeout); halMode, board, vehicle, heartBeatTimeout);
// debug serial // debug serial
hal->debug = &Serial; hal->debug = &Serial;
hal->debug->println_P(PSTR("initializing debug line")); 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 * Sensor initialization
*/ */
if (hal->getMode() == MODE_LIVE) { if (hal->getMode() == MODE_LIVE) {
hal->debug->println_P(PSTR("initializing adc")); hal->debug->println_P(PSTR("initializing adc"));
hal->adc = new ADC_CLASS; hal->adc = new ADC_CLASS;
hal->adc->Init(); hal->adc->Init();
if (gpsEnabled) { if (gpsEnabled) {
Serial1.begin(GPS_BAUD, 128, 16); // gps
hal->debug->println_P(PSTR("initializing gps")); hal->debug->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps));
hal->gps = &gpsDriver; hal->gps = &gpsDriver;
@ -128,28 +123,45 @@ void setup() {
hal->rangeFinders.push_back(rangeFinder); hal->rangeFinders.push_back(rangeFinder);
} }
} } else
/* /*
* Select guidance, navigation, control algorithms * Select guidance, navigation, control algorithms
*/ */
AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); navigator = new NAVIGATOR_CLASS(hal);
AP_Guide * guide = new GUIDE_CLASS(navigator, hal); guide = new GUIDE_CLASS(navigator, hal);
AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); controller = new CONTROLLER_CLASS(navigator, guide, hal);
/* /*
* CommLinks * CommLinks
*/ */
if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); if (board==BOARD_ARDUPILOTMEGA_2)
else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); {
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); hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
}
/* /*
* Start the autopilot * Start the autopilot
*/ */
hal->debug->printf_P(PSTR("initializing arduplane\n")); hal->debug->printf_P(PSTR("initializing arduplane\n"));
hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory());
autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal,
loop0Rate, loop1Rate, loop2Rate, loop3Rate); loop0Rate, loop1Rate, loop2Rate, loop3Rate);
} }

View File

@ -29,24 +29,19 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
hal->setState(MAV_STATE_CALIBRATING); hal->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
navigator->calibrate();
// start clock if (navigator) navigator->calibrate();
//uint32_t timeStart = millis();
//uint16_t gpsWaitTime = 5000; // 5 second wait for gps
/* /*
* Look for valid initial state * Look for valid initial state
*/ */
while (1) { while (_navigator) {
// letc gcs known we are alive // letc gcs known we are alive
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
delay(1000);
if (hal->getMode() == MODE_LIVE) { if (hal->getMode() == MODE_LIVE) {
_navigator->updateSlow(0); _navigator->updateSlow(0);
if (_hal->gps) { if (hal->gps) {
if (hal->gps->fix) { if (hal->gps->fix) {
break; break;
} else { } else {
@ -58,7 +53,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
break; break;
} }
} else if (hal->getMode() == MODE_HIL_CNTL) { // hil } 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"); Serial.println("HIL Receive Called");
if (_navigator->getTimeStamp() != 0) { if (_navigator->getTimeStamp() != 0) {
// give hil a chance to send some packets // give hil a chance to send some packets
@ -71,20 +67,24 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
break; break;
} }
hal->debug->println_P(PSTR("waiting for hil packet")); hal->debug->println_P(PSTR("waiting for hil packet"));
delay(1000);
} }
} }
AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); AP_MavlinkCommand::home.setAlt(_navigator->getAlt());
AP_MavlinkCommand::home.setLat(_navigator->getLat()); AP_MavlinkCommand::home.setLat(_navigator->getLat());
AP_MavlinkCommand::home.setLon(_navigator->getLon()); AP_MavlinkCommand::home.setLon(_navigator->getLon());
AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT);
AP_MavlinkCommand::home.save(); 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.getLat()*rad2Deg,
AP_MavlinkCommand::home.getLon()*rad2Deg); AP_MavlinkCommand::home.getLon()*rad2Deg,
AP_MavlinkCommand::home.getCommand());
AP_MavlinkCommand::home.load(); 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.getLat()*rad2Deg,
AP_MavlinkCommand::home.getLon()*rad2Deg); AP_MavlinkCommand::home.getLon()*rad2Deg,
AP_MavlinkCommand::home.getCommand());
/* /*
* Attach loops * Attach loops
@ -135,15 +135,6 @@ void AP_Autopilot::callback1(void * data) {
apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); 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 * update control laws
*/ */
@ -162,6 +153,15 @@ void AP_Autopilot::callback2(void * data) {
AP_Autopilot * apo = (AP_Autopilot *) data; AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 2")); //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 * send telemetry
*/ */

View File

@ -359,7 +359,7 @@ private:
uint32_t timeStamp = micros(); uint32_t timeStamp = micros();
switch (msg->msgid) { 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: { case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet; mavlink_heartbeat_t packet;
@ -497,7 +497,6 @@ private:
_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
AP_MavlinkCommand cmd(packet.seq); AP_MavlinkCommand cmd(packet.seq);
cmd.load();
mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,

View File

@ -40,7 +40,8 @@ public:
* @param navigator This is the navigator pointer. * @param navigator This is the navigator pointer.
*/ */
AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : 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), _headingCommand(0), _airSpeedCommand(0),
_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
_pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
@ -144,8 +145,8 @@ public:
_rangeFinderLeft(), _rangeFinderRight(), _rangeFinderLeft(), _rangeFinderRight(),
_group(k_guide, PSTR("guide_")), _group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, 1, PSTR("velCmd")), _velocityCommand(&_group, 1, 1, PSTR("velCmd")),
_crossTrackGain(&_group, 2, 2, PSTR("xt")), _crossTrackGain(&_group, 2, 1, PSTR("xt")),
_crossTrackLim(&_group, 3, 10, PSTR("xtLim")) { _crossTrackLim(&_group, 3, 90, PSTR("xtLim")) {
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i]; RangeFinder * rF = _hal->rangeFinders[i];
@ -168,67 +169,8 @@ public:
} }
virtual void update() { 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 // process mavlink commands
//handleCommand(); handleCommand();
// obstacle avoidance overrides // obstacle avoidance overrides
// stop if your going to drive into something in front of you // 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 float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc
if (_rangeFinderFront && frontDistance < 2) { if (_rangeFinderFront && frontDistance < 2) {
_mode = MAV_NAV_VECTOR; _mode = MAV_NAV_VECTOR;
//airSpeedCommand = 0; //airSpeedCommand = 0;
//groundSpeedCommand = 0; //groundSpeedCommand = 0;
// _headingCommand -= 45 * deg2Rad; // _headingCommand -= 45 * deg2Rad;
@ -291,18 +234,74 @@ public:
_previousCommand = AP_MavlinkCommand(getPreviousIndex()); _previousCommand = AP_MavlinkCommand(getPreviousIndex());
} }
void handleCommand(AP_MavlinkCommand command, void handleCommand() {
AP_MavlinkCommand previousCommand) {
// TODO handle more commands // TODO handle more commands
switch (command.getCommand()) { switch (_command.getCommand()) {
case MAV_CMD_NAV_WAYPOINT: { case MAV_CMD_NAV_WAYPOINT: {
// if within radius, increment
float d = previousCommand.distanceTo(_navigator->getLat_degInt(), // 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()); _navigator->getLon_degInt());
if (d < command.getRadius()) { 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(); nextCommand();
Serial.println("radius reached");
} }
_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; break;
} }
// case MAV_CMD_CONDITION_CHANGE_ALT: // case MAV_CMD_CONDITION_CHANGE_ALT:
@ -354,4 +353,5 @@ private:
} // namespace apo } // namespace apo
#endif // AP_Guide_H #endif // AP_Guide_H
// vim:ts=4:sw=4:expandtab // vim:ts=4:sw=4:expandtab

View File

@ -24,6 +24,19 @@ AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) :
Serial.print("key: "); Serial.println(k_commands + index); Serial.print("key: "); Serial.println(k_commands + index);
Serial.println("++"); 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 // This is a failsafe measure to stop trying to load a command if it can't load
if (doLoad && !load()) { if (doLoad && !load()) {
Serial.println("load failed, reverting to home waypoint"); Serial.println("load failed, reverting to home waypoint");
@ -157,7 +170,6 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
float bNext = previous.bearingTo(*this); float bNext = previous.bearingTo(*this);
return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth;
return 0;
} }
// calculates along track distance of a current location // calculates along track distance of a current location

View File

@ -273,13 +273,9 @@ public:
AP_Navigator::calibrate(); AP_Navigator::calibrate();
// TODO: handle cold restart // TODO: handle cold/warm restart
if (_hal->imu) { if (_hal->imu) {
/* _hal->imu->init(IMU::COLD_START,delay);
* Gyro has built in warm up cycle and should
* run first */
_hal->imu->init_gyro(NULL);
_hal->imu->init_accel(NULL);
} }
} }
virtual void updateFast(float dt) { virtual void updateFast(float dt) {