ardupilot/libraries/APO/AP_MavlinkCommand.cpp

/*
 * AP_MavlinkCommand.cpp
 *
 *  Created on: Apr 30, 2011
 *      Author: jgoppert
 */

#include "AP_MavlinkCommand.h"

namespace apo {

AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) :
    _data(v._data), _seq(v._seq) {
    //if (FastSerial::getInitialized(0)) Serial.println("copy ctor");
}

AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) :
    _data(k_commands + index), _seq(index) {

    if (FastSerial::getInitialized(0)) {
        Serial.println("index ctor");
        Serial.println("++");
        Serial.print("index: ");
        Serial.println(index);
        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");
        _data = AP_MavlinkCommand::home._data;
        _seq = AP_MavlinkCommand::home._seq;
    }
}

AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
    _data(k_commands + cmd.seq), _seq(cmd.seq) {
    setCommand(MAV_CMD(cmd.command));
    setAutocontinue(cmd.autocontinue);
    setFrame(MAV_FRAME(cmd.frame));
    setParam1(cmd.param1);
    setParam2(cmd.param2);
    setParam3(cmd.param3);
    setParam4(cmd.param4);
    setX(cmd.x);
    setY(cmd.y);
    setZ(cmd.z);
    save();

    // reload home if sent
    if (cmd.seq == 0) home.load();

    Serial.println("============================================================");
    Serial.println("storing new command from mavlink_waypoint_t");
    Serial.print("key: ");
    Serial.println(_data.key(),DEC);
    Serial.print("number: ");
    Serial.println(cmd.seq,DEC);
    Serial.print("command: ");
    Serial.println(getCommand());
    Serial.print("autocontinue: ");
    Serial.println(getAutocontinue(),DEC);
    Serial.print("frame: ");
    Serial.println(getFrame(),DEC);
    Serial.print("1000*param1: ");
    Serial.println(int(1000*getParam1()),DEC);
    Serial.print("1000*param2: ");
    Serial.println(int(1000*getParam2()),DEC);
    Serial.print("1000*param3: ");
    Serial.println(int(1000*getParam3()),DEC);
    Serial.print("1000*param4: ");
    Serial.println(int(1000*getParam4()),DEC);
    Serial.print("1000*x0: ");
    Serial.println(int(1000*cmd.x),DEC);
    Serial.print("1000*y0: ");
    Serial.println(int(1000*cmd.y),DEC);
    Serial.print("1000*z0: ");
    Serial.println(int(1000*cmd.z),DEC);
    Serial.print("1000*x: ");
    Serial.println(int(1000*getX()),DEC);
    Serial.print("1000*y: ");
    Serial.println(int(1000*getY()),DEC);
    Serial.print("1000*z: ");
    Serial.println(int(1000*getZ()),DEC);

    load();

    Serial.print("1000*x1: ");
    Serial.println(int(1000*getX()),DEC);
    Serial.print("1000*y1: ");
    Serial.println(int(1000*getY()),DEC);
    Serial.print("1000*z1: ");
    Serial.println(int(1000*getZ()),DEC);
    Serial.println("============================================================");
    Serial.flush();
}

mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) const {
    mavlink_waypoint_t mavCmd;
    mavCmd.seq = getSeq();
    mavCmd.command = getCommand();
    mavCmd.frame = getFrame();
    mavCmd.param1 = getParam1();
    mavCmd.param2 = getParam2();
    mavCmd.param3 = getParam3();
    mavCmd.param4 = getParam4();
    mavCmd.x = getX();
    mavCmd.y = getY();
    mavCmd.z = getZ();
    mavCmd.autocontinue = getAutocontinue();
    mavCmd.current = (getSeq() == current);
    mavCmd.target_component = mavlink_system.compid;
    mavCmd.target_system = mavlink_system.sysid;
    return mavCmd;
}

float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const {
    float deltaLon = next.getLon() - getLon();
    /*
     Serial.print("Lon: "); Serial.println(getLon());
     Serial.print("nextLon: "); Serial.println(next.getLon());
     Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt);
     */
    float bearing = atan2(
                        sin(deltaLon) * cos(next.getLat()),
                        cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos(
                            next.getLat()) * cos(deltaLon));
    return bearing;
}

float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const {
    // have to be careful to maintain the precision of the gps coordinate
    float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad;
    float nextLat = latDegInt * degInt2Rad;
    float bearing = atan2(
                        sin(deltaLon) * cos(nextLat),
                        cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat)
                        * cos(deltaLon));
    if (bearing < 0)
        bearing += 2 * M_PI;
    return bearing;
}

float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const {
    float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2);
    float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2);
    float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
                  next.getLat()) * sinDeltaLon2 * sinDeltaLon2;
    float c = 2 * atan2(sqrt(a), sqrt(1 - a));
    return rEarth * c;
}

float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const {
    float sinDeltaLat2 = sin(
                             (lat_degInt - getLat_degInt()) * degInt2Rad / 2);
    float sinDeltaLon2 = sin(
                             (lon_degInt - getLon_degInt()) * degInt2Rad / 2);
    float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
                  lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2;
    float c = 2 * atan2(sqrt(a), sqrt(1 - a));
    /*
     Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt());
     Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt());
     Serial.print("lat_degInt: "); Serial.println(lat_degInt);
     Serial.print("lon_degInt: "); Serial.println(lon_degInt);
     Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2);
     Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2);
     */
    return rEarth * c;
}

//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;
}

// calculates along  track distance of a current location
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
                                    int32_t lat_degInt, int32_t lon_degInt) const {
    // ignores lat/lon since single prec.
    float dXt = this->crossTrack(previous,lat_degInt, lon_degInt);
    float d = previous.distanceTo(lat_degInt, lon_degInt);
    return dXt / tan(asin(dXt / d));
}


AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false);

}