// set_home_position - all internal calculations are recorded as the distances from this point
void AP_InertialNav::set_home_position(int32_t lon, int32_t lat)
{
// set base location
_base_lon = lon;
_base_lat = lat;
// set longitude to meters scaling to offset the shrinking longitude as we go towards the poles
Location temp_loc;
temp_loc.lat = lat;
temp_loc.lng = lon;
_lon_to_cm_scaling = longitude_scale(temp_loc) * LATLON_TO_CM;
// reset corrections to base position to zero
_position_base.x = 0;
_position_base.y = 0;
_position_correction.x = 0;
_position_correction.y = 0;
_position.x = 0;
_position.y = 0;
// clear historic estimates
_hist_position_estimate_x.clear();
_hist_position_estimate_y.clear();
// set xy as enabled
_xy_enabled = true;
}
// return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
{
int32_t off_x = loc2.lng - loc1.lng;
int32_t off_y = (loc2.lat - loc1.lat) / longitude_scale(loc2);
int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000;
return bearing;
}
/*
* extrapolate latitude/longitude given distances north and east
*/
void location_offset(struct Location &loc, float ofs_north, float ofs_east)
{
if (!is_zero(ofs_north) || !is_zero(ofs_east)) {
int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(loc);
loc.lat += dlat;
loc.lng += dlng;
}
}
/*
* extrapolate latitude/longitude given distances north and east
* This function costs about 80 usec on an AVR2560
*/
void location_offset(struct Location *loc, float ofs_north, float ofs_east)
{
if (ofs_north != 0 || ofs_east != 0) {
float dlat = ofs_north * 89.831520982f;
float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc);
loc->lat += dlat;
loc->lng += dlng;
}
}
// extrapolate latitude/longitude given distances (in meters) north and east
void Location_Class::offset(float ofs_north, float ofs_east)
{
if (!is_zero(ofs_north) || !is_zero(ofs_east)) {
int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(*this);
lat += dlat;
lng += dlng;
}
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// check EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// check home is close to EKF origin
if (far_from_EKF_origin(loc)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
Log_Write_Event(DATA_SET_HOME);
#if MODE_AUTO_ENABLED == ENABLED
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
#endif
}
// lock home position
if (lock) {
ahrs.lock_home();
}
// log ahrs home and ekf origin dataflash
ahrs.Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
gcs().send_home();
gcs().send_ekf_origin();
// return success
return true;
}
// extrapolate latitude/longitude given distances (in meters) north and east
void Location::offset(float ofs_north, float ofs_east)
{
// use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class
if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) {
int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(*this);
lat += dlat;
lng += dlng;
}
}
bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{
Location ekf_origin;
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
}
vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);
return true;
}
bool Location_Class::get_vector_xy_from_origin_NEU(Vector3f &vec_neu) const
{
// convert to neu
Location ekf_origin;
if (!_ahrs->get_origin(ekf_origin)) {
return false;
}
vec_neu.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_neu.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);
return true;
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Sub::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// check if EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
Log_Write_Event(DATA_SET_HOME);
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
logger.Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// lock home position
if (lock) {
ahrs.lock_home();
}
// return success
return true;
}
// setup_home_position - reset state for home position change
void AP_InertialNav::setup_home_position(void)
{
// set longitude to meters scaling to offset the shrinking longitude as we go towards the poles
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
// reset corrections to base position to zero
_position_base.x = 0;
_position_base.y = 0;
_position_correction.x = 0;
_position_correction.y = 0;
_position.x = 0;
_position.y = 0;
// clear historic estimates
_hist_position_estimate_x.clear();
_hist_position_estimate_y.clear();
// set xy as enabled
_xy_enabled = true;
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (ap.home_state == HOME_UNSET) {
// Set compass declination automatically
if (g.compass_enabled) {
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
set_home_state(HOME_SET_NOT_LOCKED);
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// send new home location to GCS
GCS_MAVLINK::send_home_all(loc);
// return success
return true;
}
// return distance in meters between two locations
float get_distance(const struct Location &loc1, const struct Location &loc2)
{
float dlat = (float)(loc2.lat - loc1.lat);
float dlong = ((float)(loc2.lng - loc1.lng)) * longitude_scale(loc2);
return norm(dlat, dlong) * LOCATION_SCALING_FACTOR;
}
/*
return the distance in meters in North/East plane as a N/E vector
from loc1 to loc2
*/
Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
{
return Vector2f((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
(loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
}
// return distance in meters to between two locations
float get_distance(const struct Location *loc1, const struct Location *loc2)
{
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2);
return pythagorous2(dlat, dlong) * 0.01113195f;
}
