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)
: DataFlash(g.log_bitmask)
{
memset(&current_loc, 0, sizeof(current_loc));
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);
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;
}

5
AntennaTracker/defines.h
View File

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

12
AntennaTracker/system.cpp
View File

@ -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);