2011-09-28 21:51:12 -03:00
|
|
|
/*
|
|
|
|
* AP_MavlinkCommand.h
|
|
|
|
*
|
|
|
|
* Created on: Apr 4, 2011
|
|
|
|
* Author: jgoppert
|
|
|
|
*/
|
|
|
|
|
|
|
|
#ifndef AP_MAVLINKCOMMAND_H_
|
|
|
|
#define AP_MAVLINKCOMMAND_H_
|
|
|
|
|
|
|
|
#include "../GCS_MAVLink/GCS_MAVLink.h"
|
|
|
|
#include "../AP_Common/AP_Common.h"
|
|
|
|
#include "AP_Var_keys.h"
|
|
|
|
#include "constants.h"
|
|
|
|
|
|
|
|
namespace apo {
|
|
|
|
|
|
|
|
class AP_MavlinkCommand {
|
|
|
|
private:
|
2011-10-26 13:31:11 -03:00
|
|
|
struct CommandStorage {
|
|
|
|
MAV_CMD command;
|
|
|
|
bool autocontinue;
|
|
|
|
MAV_FRAME frame;
|
|
|
|
float param1;
|
|
|
|
float param2;
|
|
|
|
float param3;
|
|
|
|
float param4;
|
|
|
|
float x;
|
|
|
|
float y;
|
|
|
|
float z;
|
|
|
|
};
|
|
|
|
AP_VarS<CommandStorage> _data;
|
|
|
|
uint16_t _seq;
|
2011-09-28 21:51:12 -03:00
|
|
|
public:
|
2011-10-26 13:31:11 -03:00
|
|
|
static AP_MavlinkCommand home;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Copy Constructor
|
|
|
|
*/
|
|
|
|
AP_MavlinkCommand(const AP_MavlinkCommand & v);
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Basic Constructor
|
|
|
|
* @param index Start at zero.
|
|
|
|
*/
|
|
|
|
AP_MavlinkCommand(uint16_t index, bool doLoad = true);
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Constructor for copying/ saving from a mavlink waypoint.
|
|
|
|
* @param cmd The mavlink_waopint_t structure for the command.
|
|
|
|
*/
|
|
|
|
AP_MavlinkCommand(const mavlink_waypoint_t & cmd);
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
bool save() {
|
|
|
|
return _data.save();
|
|
|
|
}
|
|
|
|
bool load() {
|
|
|
|
return _data.load();
|
|
|
|
}
|
|
|
|
uint8_t getSeq() const {
|
|
|
|
return _seq;
|
|
|
|
}
|
|
|
|
bool getAutocontinue() const {
|
|
|
|
return _data.get().autocontinue;
|
|
|
|
}
|
|
|
|
void setAutocontinue( bool val) {
|
|
|
|
_data.get().autocontinue = val;
|
|
|
|
}
|
|
|
|
void setSeq(uint8_t val) {
|
|
|
|
_seq = val;
|
|
|
|
}
|
|
|
|
MAV_CMD getCommand() const {
|
|
|
|
return _data.get().command;
|
|
|
|
}
|
|
|
|
void setCommand(MAV_CMD val) {
|
|
|
|
_data.get().command = val;
|
|
|
|
}
|
|
|
|
MAV_FRAME getFrame() const {
|
|
|
|
return _data.get().frame;
|
|
|
|
}
|
|
|
|
void setFrame(MAV_FRAME val) {
|
|
|
|
_data.get().frame = val;
|
|
|
|
}
|
|
|
|
float getParam1() const {
|
|
|
|
return _data.get().param1;
|
|
|
|
}
|
|
|
|
void setParam1(float val) {
|
|
|
|
_data.get().param1 = val;
|
|
|
|
}
|
|
|
|
float getParam2() const {
|
|
|
|
return _data.get().param2;
|
|
|
|
}
|
|
|
|
void setParam2(float val) {
|
|
|
|
_data.get().param2 = val;
|
|
|
|
}
|
|
|
|
float getParam3() const {
|
|
|
|
return _data.get().param3;
|
|
|
|
}
|
|
|
|
void setParam3(float val) {
|
|
|
|
_data.get().param3 = val;
|
|
|
|
}
|
|
|
|
float getParam4() const {
|
|
|
|
return _data.get().param4;
|
|
|
|
}
|
|
|
|
void setParam4(float val) {
|
|
|
|
_data.get().param4 = val;
|
|
|
|
}
|
|
|
|
float getX() const {
|
|
|
|
return _data.get().x;
|
|
|
|
}
|
|
|
|
void setX(float val) {
|
|
|
|
_data.get().x = val;
|
|
|
|
}
|
|
|
|
float getY() const {
|
|
|
|
return _data.get().y;
|
|
|
|
}
|
|
|
|
void setY(float val) {
|
|
|
|
_data.get().y = val;
|
|
|
|
}
|
|
|
|
float getZ() const {
|
|
|
|
return _data.get().z;
|
|
|
|
}
|
|
|
|
void setZ(float val) {
|
|
|
|
_data.get().z = val;
|
|
|
|
}
|
2011-11-29 18:38:18 -04:00
|
|
|
|
|
|
|
float getYawCommand() const {
|
2011-11-29 19:06:38 -04:00
|
|
|
return deg2Rad*getParam4();
|
2011-11-29 18:38:18 -04:00
|
|
|
}
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getLatDeg() const {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
return getX();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void setLatDeg(float val) {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
setX(val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
float getLonDeg() const {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
return getY();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void setLonDeg(float val) {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
setY(val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void setLon(float val) {
|
|
|
|
setLonDeg(val * rad2Deg);
|
|
|
|
}
|
|
|
|
void setLon_degInt(int32_t val) {
|
|
|
|
setLonDeg(val / 1.0e7);
|
|
|
|
}
|
|
|
|
void setLat_degInt(int32_t val) {
|
|
|
|
setLatDeg(val / 1.0e7);
|
|
|
|
}
|
|
|
|
int32_t getLon_degInt() const {
|
|
|
|
return getLonDeg() * 1e7;
|
|
|
|
}
|
|
|
|
int32_t getLat_degInt() const {
|
|
|
|
return getLatDeg() * 1e7;
|
|
|
|
}
|
|
|
|
float getLon() const {
|
|
|
|
return getLonDeg() * deg2Rad;
|
|
|
|
}
|
|
|
|
float getLat() const {
|
|
|
|
return getLatDeg() * deg2Rad;
|
|
|
|
}
|
|
|
|
void setLat(float val) {
|
|
|
|
setLatDeg(val * rad2Deg);
|
|
|
|
}
|
|
|
|
float getAlt() const {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
return getZ();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
return -getZ() + home.getAlt();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
return getZ() + home.getAlt();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
|
|
* set the altitude in meters
|
|
|
|
*/
|
|
|
|
void setAlt(float val) {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
setZ(val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
setZ(home.getLonDeg() - val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
setZ(val - home.getLonDeg());
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
|
|
* Get the relative altitude to home
|
|
|
|
* @return relative altitude in meters
|
|
|
|
*/
|
|
|
|
float getRelAlt() const {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
return getZ() - home.getAlt();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
return -getZ();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
return getZ();
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
|
|
* set the relative altitude in meters from home (up)
|
|
|
|
*/
|
|
|
|
void setRelAlt(float val) {
|
|
|
|
switch (getFrame()) {
|
|
|
|
case MAV_FRAME_GLOBAL:
|
|
|
|
setZ(val + home.getAlt());
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
|
|
|
|
case MAV_FRAME_LOCAL:
|
|
|
|
setZ(-val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_LOCAL_ENU:
|
|
|
|
setZ(val);
|
|
|
|
break;
|
|
|
|
case MAV_FRAME_MISSION:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getRadius() const {
|
|
|
|
return getParam2();
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
void setRadius(float val) {
|
|
|
|
setParam2(val);
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* conversion for outbound packets to ground station
|
|
|
|
* @return output the mavlink_waypoint_t packet
|
|
|
|
*/
|
|
|
|
mavlink_waypoint_t convert(uint8_t current) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Calculate the bearing from this command to the next command
|
|
|
|
* @param next The command to calculate the bearing to.
|
|
|
|
* @return the bearing
|
|
|
|
*/
|
|
|
|
float bearingTo(const AP_MavlinkCommand & next) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Bearing form this command to a gps coordinate in integer units
|
|
|
|
* @param latDegInt latitude in degrees E-7
|
|
|
|
* @param lonDegInt longitude in degrees E-7
|
|
|
|
* @return
|
|
|
|
*/
|
|
|
|
float bearingTo(int32_t latDegInt, int32_t lonDegInt) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Distance to another command
|
|
|
|
* @param next The command to measure to.
|
|
|
|
* @return The distance in meters.
|
|
|
|
*/
|
|
|
|
float distanceTo(const AP_MavlinkCommand & next) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Distance to a gps coordinate in integer units
|
|
|
|
* @param latDegInt latitude in degrees E-7
|
|
|
|
* @param lonDegInt longitude in degrees E-7
|
|
|
|
* @return The distance in meters.
|
|
|
|
*/
|
|
|
|
float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getPN(int32_t lat_degInt, int32_t lon_degInt) const {
|
|
|
|
// local tangent approximation at this waypoint
|
|
|
|
float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad;
|
|
|
|
return deltaLat * rEarth;
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getPE(int32_t lat_degInt, int32_t lon_degInt) const {
|
|
|
|
// local tangent approximation at this waypoint
|
|
|
|
float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad;
|
|
|
|
return cos(getLat()) * deltaLon * rEarth;
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getPD(int32_t alt_intM) const {
|
|
|
|
return -(alt_intM / scale_m - getAlt());
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getLat(float pN) const {
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
return pN / rEarth + getLat();
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
float getLon(float pE) const {
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
return pE / rEarth / cos(getLat()) + getLon();
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
/**
|
|
|
|
* Gets altitude in meters
|
|
|
|
* @param pD alt in meters
|
|
|
|
* @return
|
|
|
|
*/
|
|
|
|
float getAlt(float pD) const {
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
return getAlt() - pD;
|
|
|
|
}
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
//calculates cross track of a current location
|
|
|
|
float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// calculates along track distance of a current location
|
2011-11-29 18:37:42 -04:00
|
|
|
float alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next, int32_t lat_degInt, int32_t lon_degInt) const;
|
2011-09-28 21:51:12 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
} // namespace apo
|
|
|
|
|
|
|
|
|
|
|
|
#endif /* AP_MAVLINKCOMMAND_H_ */
|
2011-10-26 15:59:40 -03:00
|
|
|
// vim:ts=4:sw=4:expandtab
|