2011-09-28 21:51:12 -03:00
|
|
|
/*
|
|
|
|
* AP_Guide.cpp
|
|
|
|
*
|
|
|
|
* Created on: Apr 30, 2011
|
|
|
|
* Author: jgoppert
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "AP_Guide.h"
|
2011-10-16 03:55:34 -03:00
|
|
|
#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),
|
2011-11-20 00:34:14 -04:00
|
|
|
_groundSpeedCommand(0), _altitudeCommand(0),
|
|
|
|
_mode(MAV_NAV_LOST),
|
2011-10-26 13:31:11 -03:00
|
|
|
_numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
|
|
|
|
_nextCommandTimer(0) {
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
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-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2011-10-16 03:55:34 -03:00
|
|
|
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
|
2011-10-26 13:31:11 -03:00
|
|
|
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
|
2011-11-01 15:09:57 -03:00
|
|
|
AP_Guide(navigator, hal),
|
2011-10-26 13:31:11 -03:00
|
|
|
_group(k_guide, PSTR("guide_")),
|
|
|
|
_velocityCommand(&_group, 1, velCmd, PSTR("velCmd")),
|
|
|
|
_crossTrackGain(&_group, 2, xt, PSTR("xt")),
|
|
|
|
_crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) {
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkGuide::update() {
|
2011-10-26 13:31:11 -03:00
|
|
|
// process mavlink commands
|
|
|
|
handleCommand();
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
2011-11-20 00:34:14 -04:00
|
|
|
float MavlinkGuide::getPNError() {
|
|
|
|
return -_command.getPN(_navigator->getLat_degInt(), _navigator->getLon_degInt());
|
|
|
|
}
|
|
|
|
float MavlinkGuide::getPEError() {
|
|
|
|
return -_command.getPE(_navigator->getLat_degInt(), _navigator->getLon_degInt());
|
|
|
|
}
|
|
|
|
float MavlinkGuide::getPDError() {
|
|
|
|
return -_command.getPD(_navigator->getAlt_intM());
|
|
|
|
}
|
|
|
|
|
2011-10-16 03:55:34 -03:00
|
|
|
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
|
2011-10-27 18:23:08 -03:00
|
|
|
setCurrentIndex(getNextIndex());
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
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-16 03:55:34 -03:00
|
|
|
|
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
|
2011-11-19 22:39:14 -04:00
|
|
|
if ( (distanceToNext < _command.getRadius()) | (distanceToNext < 1e-5) ) {
|
2011-10-27 20:42:57 -03:00
|
|
|
_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;
|
|
|
|
|
|
|
|
// debug
|
2011-11-19 22:39:14 -04:00
|
|
|
//_hal->debug->printf_P(
|
|
|
|
//PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
|
|
|
|
//getNumberOfCommands(), getCurrentIndex(), getPreviousIndex());
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
} // namespace apo
|
|
|
|
|
|
|
|
// vim:ts=4:sw=4:expandtab
|