// guided_limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command bool Copter::guided_limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { return true; } // get current location const Vector3f& curr_pos = inertial_nav.get_position(); // check if we have gone below min alt if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { return true; } // check if we have gone above max alt if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) { return true; } // check if we have gone beyond horizontal limit if (guided_limit.horiz_max_cm > 0.0f) { float horiz_move = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos); if (horiz_move > guided_limit.horiz_max_cm) { return true; } } // if we got this far we must be within limits return false; }
// fence_check - ask fence library to check for breaches and initiate the response // called at 1hz void Sub::fence_check() { uint8_t new_breaches; // the type of fence that has been breached uint8_t orig_breaches = fence.get_breaches(); Vector3f home = pv_location_to_vector(ahrs.get_home()); Vector3f curr = inertial_nav.get_position(); int32_t home_distance = pv_get_horizontal_distance_cm(curr, home); // give fence library our current distance from home in meters fence.set_home_distance(home_distance*0.01f); // check for a breach new_breaches = fence.check_fence(current_loc.alt/100.0f); // return immediately if motors are not armed if (!motors.armed()) { return; } // if there is a new breach take action if (new_breaches != AC_FENCE_TYPE_NONE) { // if the user wants some kind of response and motors are armed if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { // // // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle // // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down // if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){ // init_disarm_motors(); // }else{ // // if we are within 100m of the fence, RTL // if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { // if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) { // set_mode(LAND, MODE_REASON_FENCE_BREACH); // } // }else{ // // if more than 100m outside the fence just force a land // set_mode(LAND, MODE_REASON_FENCE_BREACH); // } // } } // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches); } // record clearing of breach if (orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) { Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED); } }
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions void Copter::calc_home_distance_and_bearing() { // calculate home distance and bearing if (position_ok()) { Vector3f home = pv_location_to_vector(ahrs.get_home()); Vector3f curr = inertial_nav.get_position(); home_distance = pv_get_horizontal_distance_cm(curr, home); home_bearing = pv_get_bearing_cd(curr,home); // update super simple bearing (if required) because it relies on home_bearing update_super_simple_bearing(false); } }
// returns distance between a destination and home in cm float Sub::pv_distance_to_home_cm(const Vector3f &destination) { Vector3f home = pv_location_to_vector(ahrs.get_home()); return pv_get_horizontal_distance_cm(home, destination); }