Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: adjust for Location_Class and Location unification

This commit is contained in:
Peter Barker 2019-01-02 14:24:53 +11:00 committed by Peter Barker
parent e3c719b27f
commit 61df15ffe8
4 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
AntennaTracker/AntennaTracker.cpp
View File

@ -141,7 +141,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
Tracker::Tracker(void) Tracker::Tracker(void)
: DataFlash(g.log_bitmask) : DataFlash(g.log_bitmask)
{ {
memset(&current_loc, 0, sizeof(current_loc));
memset(&vehicle, 0, sizeof(vehicle)); memset(&vehicle, 0, sizeof(vehicle));
} }

44
AntennaTracker/GCS_Mavlink.cpp
View File

@ -450,28 +450,31 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
mavlink_msg_mission_item_decode(msg, &packet); mavlink_msg_mission_item_decode(msg, &packet);
struct Location tell_command = {}; struct Location tell_command;
switch (packet.frame) switch (packet.frame)
{ {
case MAV_FRAME_MISSION: case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
{ {
tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command = Location{
tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 int32_t(1.0e7f*packet.x), // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2f; // in as m converted to cm int32_t(1.0e7f*packet.y), // in as DD converted to * t7
tell_command.options = 0; // absolute altitude int32_t(packet.z*1.0e2f), // in as m converted to cm
Location::ALT_FRAME_ABSOLUTE
};
break; break;
} }
#ifdef MAV_FRAME_LOCAL_NED #ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position) case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{ {
tell_command.lat = 1.0e7f*ToDeg(packet.x/ tell_command = Location{
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; int32_t(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; int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng),
tell_command.alt = -packet.z*1.0e2f; int32_t(-packet.z*1.0e2f),
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; Location::ALT_FRAME_ABOVE_HOME
};
break; break;
} }
#endif #endif
@ -479,21 +482,24 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
#ifdef MAV_FRAME_LOCAL #ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position) case MAV_FRAME_LOCAL: // local (relative to home position)
{ {
tell_command.lat = 1.0e7f*ToDeg(packet.x/ tell_command = {
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; int32_t(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; int32_t(1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng),
tell_command.alt = packet.z*1.0e2f; int32_t(packet.z*1.0e2f),
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; Location::ALT_FRAME_ABOVE_HOME
};
break; break;
} }
#endif #endif
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude 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 = {
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 int32_t(1.0e7f * packet.x), // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f; int32_t(1.0e7f * packet.y), // in as DD converted to * t7
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! int32_t(packet.z * 1.0e2f),
Location::ALT_FRAME_ABOVE_HOME
};
break; break;
} }

5
AntennaTracker/defines.h
View File

@ -1,10 +1,5 @@
#pragma once #pragma once
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative
// altitude
// Controller modes // Controller modes
// ---------------- // ----------------

12
AntennaTracker/system.cpp
View File

@ -127,17 +127,19 @@ bool Tracker::get_home_eeprom(struct Location &loc)
} }
// read WP position // read WP position
loc.options = wp_storage.read_byte(0); loc = {
loc.alt = wp_storage.read_uint32(1); int32_t(wp_storage.read_uint32(5)),
loc.lat = wp_storage.read_uint32(5); int32_t(wp_storage.read_uint32(9)),
loc.lng = wp_storage.read_uint32(9); int32_t(wp_storage.read_uint32(1)),
Location::ALT_FRAME_ABSOLUTE
};
return true; return true;
} }
void Tracker::set_home_eeprom(struct Location temp) 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(1, temp.alt);
wp_storage.write_uint32(5, temp.lat); wp_storage.write_uint32(5, temp.lat);
wp_storage.write_uint32(9, temp.lng); wp_storage.write_uint32(9, temp.lng);