// set_auto_yaw_roi - sets the yaw to look at roi for auto mode void Copter::set_auto_yaw_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command set_auto_yaw_mode(get_default_auto_yaw_mode(false)); #if MOUNT == ENABLED // switch off the camera tracking if enabled if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { camera_mount.set_mode_to_default(); } #endif // MOUNT == ENABLED }else{ #if MOUNT == ENABLED // check if mount type requires us to rotate the quad if(!camera_mount.has_pan_control()) { roi_WP = pv_location_to_vector(roi_location); set_auto_yaw_mode(AUTO_YAW_ROI); } // send the command to the camera mount camera_mount.set_roi_target(roi_location); // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) // 0: do nothing // 1: point at next waypoint // 2: point at a waypoint taken from WP# parameter (2nd parameter?) // 3: point at a location given by alt, lon, lat parameters // 4: point at a target given a target id (can't be implemented) #else // if we have no camera mount aim the quad at the location roi_WP = pv_location_to_vector(roi_location); set_auto_yaw_mode(AUTO_YAW_ROI); #endif // MOUNT == ENABLED } }
void Copter::rtl_build_path() { // origin point is our stopping point pos_control.get_stopping_point_xy(rtl_path.origin_point); pos_control.get_stopping_point_z(rtl_path.origin_point); // set return target to nearest rally point or home position #if AC_RALLY == ENABLED Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0); rtl_path.return_target = pv_location_to_vector(rally_point); #else rtl_path.return_target = pv_location_to_vector(ahrs.get_home()); #endif Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point; float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y); // compute return altitude rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist); // climb target is above our origin point at the return altitude rtl_path.climb_target.x = rtl_path.origin_point.x; rtl_path.climb_target.y = rtl_path.origin_point.y; rtl_path.climb_target.z = rtl_path.return_target.z; // descent target is below return target at rtl_alt_final rtl_path.descent_target.x = rtl_path.return_target.x; rtl_path.descent_target.y = rtl_path.return_target.y; rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final); // set land flag rtl_path.land = g.rtl_alt_final <= 0; }
// verify_circle - check if we have circled the point enough bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) { // check if we've reached the edge if (auto_mode == Auto_CircleMoveToEdge) { if (wp_nav.reached_wp_destination()) { Vector3f curr_pos = inertial_nav.get_position(); Vector3f circle_center = pv_location_to_vector(cmd.content.location); // set target altitude if not provided if (is_zero(circle_center.z)) { circle_center.z = curr_pos.z; } // set lat/lon position if not provided if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { circle_center.x = curr_pos.x; circle_center.y = curr_pos.y; } // start circling auto_circle_start(); } return false; } // check if we have completed circling return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); }
// do_guided - start guided mode bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { Vector3f pos_or_vel; // target location or velocity // only process guided waypoint if we are in guided mode if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { return false; } // switch to handle different commands switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: // set wp_nav's destination pos_or_vel = pv_location_to_vector(cmd.content.location); guided_set_destination(pos_or_vel); return true; break; case MAV_CMD_CONDITION_YAW: do_yaw(cmd); return true; break; default: // reject unrecognised command return false; break; } return true; }
// do_loiter_time - initiate loitering at a point for a given time period // note: caller should set yaw_mode void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd) { Vector3f target_pos; // get current position Vector3f curr_pos = inertial_nav.get_position(); // default to use position provided target_pos = pv_location_to_vector(cmd.content.location); // use current location if not provided if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { wp_nav.get_wp_stopping_point_xy(target_pos); } // use current altitude if not provided if( cmd.content.location.alt == 0 ) { target_pos.z = curr_pos.z; } // start way point navigator and provide it the desired location auto_wp_start(target_pos); // setup loiter timer loiter_time = 0; loiter_time_max = cmd.p1; // units are (seconds) }
// 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); } }
// rtl_return_start - initialise return to home void Copter::rtl_return_start() { rtl_state = RTL_ReturnHome; rtl_state_complete = false; // set target to above home/rally point #if AC_RALLY == ENABLED // rally_point will be the nearest rally point or home. uses absolute altitudes Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home Vector3f destination = pv_location_to_vector(rally_point); #else Vector3f destination = pv_location_to_vector(ahrs.get_home()); destination.z = pv_alt_above_origin(rtl_alt)); #endif wp_nav.set_wp_destination(destination); // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); }
// 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); } }
// do_land - initiate landing procedure void Copter::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed // if location provided we fly to that location at current altitude if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { // set state to fly to location land_state = LAND_STATE_FLY_TO_LOCATION; // calculate and set desired location above landing target Vector3f pos = pv_location_to_vector(cmd.content.location); pos.z = inertial_nav.get_altitude(); auto_wp_start(pos); }else{ // set landing state land_state = LAND_STATE_DESCENDING; // initialise landing controller auto_land_start(); } }
// do_circle - initiate moving in a circle void Copter::do_circle(const AP_Mission::Mission_Command& cmd) { Vector3f curr_pos = inertial_nav.get_position(); Vector3f circle_center = pv_location_to_vector(cmd.content.location); uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 bool move_to_edge_required = false; // set target altitude if not provided if (cmd.content.location.alt == 0) { circle_center.z = curr_pos.z; } else { move_to_edge_required = true; } // set lat/lon position if not provided // To-Do: use previous command's destination if it was a straight line or spline waypoint command if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { circle_center.x = curr_pos.x; circle_center.y = curr_pos.y; } else { move_to_edge_required = true; } // set circle controller's center circle_nav.set_center(circle_center); // set circle radius if (circle_radius_m != 0) { circle_nav.set_radius((float)circle_radius_m * 100.0f); } // check if we need to move to edge of circle if (move_to_edge_required) { // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge auto_circle_movetoedge_start(); } else { // start circling auto_circle_start(); } }
// do_loiter_unlimited - start loitering with no end conditions // note: caller should set yaw_mode void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { Vector3f target_pos; // get current position Vector3f curr_pos = inertial_nav.get_position(); // default to use position provided target_pos = pv_location_to_vector(cmd.content.location); // use current location if not provided if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { wp_nav.get_wp_stopping_point_xy(target_pos); } // use current altitude if not provided // To-Do: use z-axis stopping point instead of current alt if( cmd.content.location.alt == 0 ) { target_pos.z = curr_pos.z; } // start way point navigator and provide it the desired location auto_wp_start(target_pos); }
// 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); }