// read_inertia - read inertia in from accelerometers
void Copter::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
return;
}
// without home return alt above the EKF origin
if (!ahrs.home_is_set()) {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = inertial_nav.get_altitude();
} else {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude());
}
// set flags and get velocity
current_loc.relative_alt = true;
climb_rate = inertial_nav.get_velocity_z();
}
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
void Copter::read_inertial_altitude()
{
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
return;
}
// without home return alt above the EKF origin
if (ap.home_state == HOME_UNSET) {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = inertial_nav.get_altitude();
} else {
// with inertial nav we can update the altitude and climb rate at 50hz
current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude());
}
// set flags and get velocity
current_loc.flags.relative_alt = true;
climb_rate = inertial_nav.get_velocity_z();
}
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Copter::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
