2011-03-19 07:20:11 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
// run this at setup on the ground
|
|
|
|
// -------------------------------
|
2011-07-17 07:32:00 -03:00
|
|
|
static void init_home()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2012-11-10 01:55:51 -04:00
|
|
|
set_home_is_set(true);
|
2014-03-31 04:07:46 -03:00
|
|
|
|
|
|
|
// copter uses 0 home altitude
|
|
|
|
Location loc = gps.location();
|
|
|
|
|
|
|
|
ahrs.set_home(loc);
|
2012-08-16 21:50:03 -03:00
|
|
|
|
2014-01-03 07:41:23 -04:00
|
|
|
inertial_nav.setup_home_position();
|
2012-11-07 06:03:30 -04:00
|
|
|
|
2014-02-27 22:16:33 -04:00
|
|
|
// log new home position which mission library will pull from ahrs
|
2014-10-16 21:37:49 -03:00
|
|
|
if (should_log(MASK_LOG_CMD)) {
|
2014-02-27 22:16:33 -04:00
|
|
|
AP_Mission::Mission_Command temp_cmd;
|
2014-03-10 23:27:53 -03:00
|
|
|
if (mission.read_cmd_from_storage(0, temp_cmd)) {
|
2014-03-16 05:41:59 -03:00
|
|
|
Log_Write_Cmd(temp_cmd);
|
2014-03-10 23:27:53 -03:00
|
|
|
}
|
2014-02-27 22:16:33 -04:00
|
|
|
}
|
2012-08-16 21:50:03 -03:00
|
|
|
|
2012-12-17 03:54:03 -04:00
|
|
|
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
2014-06-10 23:57:30 -03:00
|
|
|
scaleLongDown = longitude_scale(loc);
|
2013-01-27 10:35:12 -04:00
|
|
|
scaleLongUp = 1.0f/scaleLongDown;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2015-01-05 00:51:20 -04:00
|
|
|
// update_home - reset home to current location
|
|
|
|
// should be called only when vehicle is disarmed
|
|
|
|
static void update_home()
|
|
|
|
{
|
|
|
|
// copter uses 0 home altitude
|
|
|
|
Location loc = gps.location();
|
|
|
|
|
|
|
|
// set ahrs object's home position
|
|
|
|
ahrs.set_home(loc);
|
|
|
|
|
|
|
|
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
|
|
|
scaleLongDown = longitude_scale(loc);
|
|
|
|
scaleLongUp = 1.0f/scaleLongDown;
|
|
|
|
}
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
|