// 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; }