/* * AP_MavlinkCommand.cpp * * Created on: Apr 30, 2011 * Author: jgoppert */ #include "../FastSerial/FastSerial.h" #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, home must be a global waypoint if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) 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 { float dXt = crossTrack(previous,lat_degInt,lon_degInt); float d = previous.distanceTo(getLat_degInt(),getLon_degInt()); float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); float bNext = previous.bearingTo(*this); float y = acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth; if (cos(bCurrent-bNext) < 0) y = -y; return y; } AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false); } // namespace apo // vim:ts=4:sw=4:expandtab