ardupilot/libraries/APO/AP_Guide.cpp

283 lines
9.7 KiB
C++
Raw Normal View History

2011-09-28 21:51:12 -03:00
/*
* AP_Guide.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#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) :
2011-10-26 13:31:11 -03:00
_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) {
}
2011-10-26 13:31:11 -03:00
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);
2011-10-27 20:42:57 -03:00
updateCommand();
}
2011-10-26 14:25:06 -03:00
float AP_Guide::getHeadingError() {
float headingError = getHeadingCommand()
- _navigator->getYaw();
if (headingError > 180 * deg2Rad)
headingError -= 360 * deg2Rad;
if (headingError < -180 * deg2Rad)
headingError += 360 * deg2Rad;
return headingError;
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
2011-10-26 13:31:11 -03:00
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(),
_rangeFinderLeft(), _rangeFinderRight(),
_group(k_guide, PSTR("guide_")),
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) {
RangeFinder * rF = _hal->rangeFinders[i];
if (rF == NULL)
continue;
if (rF->orientation_x == 1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderFront = rF;
else if (rF->orientation_x == -1 && rF->orientation_y == 0
&& rF->orientation_z == 0)
_rangeFinderBack = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == 1
&& rF->orientation_z == 0)
_rangeFinderRight = rF;
else if (rF->orientation_x == 0 && rF->orientation_y == -1
&& rF->orientation_z == 0)
_rangeFinderLeft = rF;
}
}
void MavlinkGuide::update() {
2011-10-26 13:31:11 -03:00
// 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);
2011-10-26 13:31:11 -03:00
}
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() {
2011-10-26 13:31:11 -03:00
// 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;
}
2011-10-27 20:42:57 -03:00
// set the current command
setCurrentIndex(getNextIndex());
}
2011-10-27 20:42:57 -03:00
void MavlinkGuide::updateCommand() {
// update guidance mode
if (_command.getCommand() == MAV_CMD_NAV_WAYPOINT) {
_mode = MAV_NAV_WAYPOINT;
} else if (_command.getCommand() == MAV_CMD_NAV_LAND) {
_mode = MAV_NAV_LANDING;
} else if (_command.getCommand() == MAV_CMD_NAV_LOITER_TIME) {
_mode = MAV_NAV_LOITER;
} else if (_command.getCommand() == MAV_CMD_NAV_LOITER_UNLIM) {
_mode = MAV_NAV_LOITER;
} else if (_command.getCommand() == MAV_CMD_NAV_LOITER_TURNS) {
_mode = MAV_NAV_LOITER;
} else if (_command.getCommand() == MAV_CMD_NAV_RETURN_TO_LAUNCH) {
_mode = MAV_NAV_RETURNING;
} else if (_command.getCommand() == MAV_CMD_NAV_TAKEOFF) {
_mode = MAV_NAV_LIFTOFF;
} else {
_hal->debug->printf_P(PSTR("unhandled command"));
_hal->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled command"));
nextCommand();
return;
}
2011-10-26 13:31:11 -03:00
// TODO handle more commands
2011-10-27 20:42:57 -03:00
//MAV_CMD_CONDITION_CHANGE_ALT
//MAV_CMD_CONDITION_DELAY
//MAV_CMD_CONDITION_DISTANCE
//MAV_CMD_CONDITION_LAST
//MAV_CMD_CONDITION_YAW
2011-10-26 13:31:11 -03:00
2011-10-27 20:42:57 -03:00
//MAV_CMD_DO_CHANGE_SPEED
//MAV_CMD_DO_CONTROL_VIDEO
//MAV_CMD_DO_JUMP
//MAV_CMD_DO_LAST
//MAV_CMD_DO_LAST
//MAV_CMD_DO_REPEAT_RELAY
//MAV_CMD_DO_REPEAT_SERVO
//MAV_CMD_DO_SET_HOME
//MAV_CMD_DO_SET_MODE
//MAV_CMD_DO_SET_PARAMETER
//MAV_CMD_DO_SET_RELAY
//MAV_CMD_DO_SET_SERVO
//MAV_CMD_PREFLIGHT_CALIBRATION
//MAV_CMD_PREFLIGHT_STORAGE
}
void MavlinkGuide::handleCommand() {
// for these modes use crosstrack navigation
if (
_mode == MAV_NAV_WAYPOINT ||
_mode == MAV_NAV_LANDING ||
_mode == MAV_NAV_LIFTOFF ||
_mode == MAV_NAV_VECTOR) {
2011-10-26 13:31:11 -03:00
// if we don't have enough waypoint for cross track calcs
2011-10-27 20:42:57 -03:00
// switch to loiter mode
2011-10-26 13:31:11 -03:00
if (_numberOfCommands == 1) {
2011-10-27 20:42:57 -03:00
_mode = MAV_NAV_LOITER;
return;
2011-10-26 13:31:11 -03:00
}
2011-10-27 20:42:57 -03:00
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
2011-10-26 13:31:11 -03:00
2011-10-27 20:42:57 -03:00
// check if we are at waypoint or if command
// radius is zero within tolerance
if (distanceToNext < _command.getRadius() | distanceToNext < 1e-5) {
_hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)"));
_hal->debug->printf_P(PSTR("waypoint reached (distance)"));
nextCommand();
return;
}
// check for along track next waypoint requirement
float alongTrack = _command.alongTrack(_previousCommand,
_navigator->getLat_degInt(),
_navigator->getLon_degInt());
float segmentLength = _previousCommand.distanceTo(_command);
if (alongTrack > segmentLength) {
_hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (along track)"));
_hal->debug->printf_P(PSTR("waypoint reached (along track)"));
nextCommand();
return;
}
2011-10-26 13:31:11 -03:00
2011-10-27 20:42:57 -03:00
// calculate altitude and heading commands
_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;
2011-10-26 13:31:11 -03:00
//_hal->debug->printf_P(
2011-10-27 20:42:57 -03:00
// PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
// bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);
// for these modes just head to current command
} else if (
_mode == MAV_NAV_LOITER ||
_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;
// do nothing for these modes
} else if (
_mode == MAV_NAV_GROUNDED ||
_mode == MAV_NAV_HOLD ||
_mode == MAV_NAV_LOST) {
2011-10-26 13:31:11 -03:00
}
2011-10-27 20:42:57 -03:00
// if in unhandled mode, then return
else {
_hal->debug->printf_P(PSTR("unhandled guide mode"));
_hal->gcs->sendText(SEVERITY_HIGH,PSTR("unhandled guide mode"));
_mode = MAV_NAV_RETURNING;
2011-10-26 13:31:11 -03:00
}
2011-10-27 20:42:57 -03:00
_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());
}
} // namespace apo
// vim:ts=4:sw=4:expandtab