diff --git a/AntennaTracker/AntennaTracker.cpp b/AntennaTracker/AntennaTracker.cpp index d1b14de1b8..0864c06e4d 100644 --- a/AntennaTracker/AntennaTracker.cpp +++ b/AntennaTracker/AntennaTracker.cpp @@ -141,7 +141,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Tracker::Tracker(void) : DataFlash(g.log_bitmask) { - memset(¤t_loc, 0, sizeof(current_loc)); memset(&vehicle, 0, sizeof(vehicle)); } diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index df961c7508..8a3d01564c 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -450,28 +450,31 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg) mavlink_msg_mission_item_decode(msg, &packet); - struct Location tell_command = {}; + struct Location tell_command; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { - tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2f; // in as m converted to cm - tell_command.options = 0; // absolute altitude + tell_command = Location{ + int32_t(1.0e7f*packet.x), // in as DD converted to * t7 + int32_t(1.0e7f*packet.y), // in as DD converted to * t7 + int32_t(packet.z*1.0e2f), // in as m converted to cm + Location::ALT_FRAME_ABSOLUTE + }; break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { - tell_command.lat = 1.0e7f*ToDeg(packet.x/ - (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; - tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; - tell_command.alt = -packet.z*1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + tell_command = Location{ + int32_t(1.0e7f*ToDeg(packet.x/(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat), + int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng), + int32_t(-packet.z*1.0e2f), + Location::ALT_FRAME_ABOVE_HOME + }; break; } #endif @@ -479,21 +482,24 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg) #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { - tell_command.lat = 1.0e7f*ToDeg(packet.x/ - (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; - tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; - tell_command.alt = packet.z*1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + tell_command = { + int32_t(1.0e7f*ToDeg(packet.x/(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat), + int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng), + int32_t(packet.z*1.0e2f), + Location::ALT_FRAME_ABOVE_HOME + }; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { - tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2f; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + tell_command = { + int32_t(1.0e7f * packet.x), // in as DD converted to * t7 + int32_t(1.0e7f * packet.y), // in as DD converted to * t7 + int32_t(packet.z * 1.0e2f), + Location::ALT_FRAME_ABOVE_HOME + }; break; } diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h index 234cec0f84..830b13fe40 100644 --- a/AntennaTracker/defines.h +++ b/AntennaTracker/defines.h @@ -1,10 +1,5 @@ #pragma once -// Command/Waypoint/Location Options Bitmask -//-------------------- -#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative - // altitude - // Controller modes // ---------------- diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 6cf267e95c..9a4986256f 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -127,17 +127,19 @@ bool Tracker::get_home_eeprom(struct Location &loc) } // read WP position - loc.options = wp_storage.read_byte(0); - loc.alt = wp_storage.read_uint32(1); - loc.lat = wp_storage.read_uint32(5); - loc.lng = wp_storage.read_uint32(9); + loc = { + int32_t(wp_storage.read_uint32(5)), + int32_t(wp_storage.read_uint32(9)), + int32_t(wp_storage.read_uint32(1)), + Location::ALT_FRAME_ABSOLUTE + }; return true; } void Tracker::set_home_eeprom(struct Location temp) { - wp_storage.write_byte(0, temp.options); + wp_storage.write_byte(0, 0); wp_storage.write_uint32(1, temp.alt); wp_storage.write_uint32(5, temp.lat); wp_storage.write_uint32(9, temp.lng);