// 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 } }
// rtl_loiterathome_start - initialise return to home void Copter::rtl_loiterathome_start() { rtl_state = RTL_LoiterAtHome; rtl_state_complete = false; rtl_loiter_start_time = millis(); // yaw back to initial take-off heading yaw unless pilot has already overridden yaw if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); } else { set_auto_yaw_mode(AUTO_YAW_HOLD); } }
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has performed all required GPS_ok checks void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m) { // convert location to vector from ekf origin Vector3f circle_center_neu; if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) { // default to current position and log error circle_center_neu = inertial_nav.get_position(); Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT); } circle_nav.set_center(circle_center_neu); // set circle radius if (!is_zero(radius_m)) { circle_nav.set_radius(radius_m * 100.0f); } // check our distance from edge of circle Vector3f circle_edge_neu; circle_nav.get_closest_point_on_circle(circle_edge_neu); float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length(); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle auto_mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location_Class Location_Class circle_edge(circle_edge_neu); // convert altitude to same as command circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); // initialise wpnav to move to edge of circle if (!wp_nav.set_wp_destination(circle_edge)) { // failure to set destination can only be because of missing terrain data failsafe_terrain_on_event(); } // if we are outside the circle, point at the edge, otherwise hold yaw const Vector3f &curr_pos = inertial_nav.get_position(); float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle set_auto_yaw_mode(AUTO_YAW_HOLD); } } else { auto_circle_start(); } }
// initialise guided mode's angle controller void Copter::guided_angle_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Angle; // set vertical speed and acceleration pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); pos_control.set_accel_z(wp_nav.get_accel_z()); // initialise position and desired velocity if (!pos_control.is_active_z()) { pos_control.set_alt_target_to_current_alt(); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); } // initialise targets guided_angle_state.update_time_ms = millis(); guided_angle_state.roll_cd = ahrs.roll_sensor; guided_angle_state.pitch_cd = ahrs.pitch_sensor; guided_angle_state.yaw_cd = ahrs.yaw_sensor; guided_angle_state.climb_rate_cms = 0.0f; guided_angle_state.yaw_rate_cds = 0.0f; guided_angle_state.use_yaw_rate = false; // pilot always controls yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }
// rtl_climb_start - initialise climb to RTL altitude void Copter::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; rtl_alt = get_RTL_alt(); // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // get horizontal stopping point Vector3f destination; wp_nav.get_wp_stopping_point_xy(destination); #if AC_RALLY == ENABLED // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. 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 destination.z = pv_alt_above_origin(rally_point.alt); #else destination.z = pv_alt_above_origin(rtl_alt); #endif // set the destination wp_nav.set_wp_destination(destination); wp_nav.set_fast_waypoint(true); // hold current yaw during initial climb set_auto_yaw_mode(AUTO_YAW_HOLD); }
// guided_takeoff_start - initialises waypoint controller to implement take-off bool Copter::guided_takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; // initialise wpnav destination Location_Class target_loc = current_loc; target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); if (!wp_nav.set_wp_destination(target_loc)) { // failure to set destination can only be because of missing terrain data Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); // get initial alt for WP_NAVALT_MIN auto_takeoff_set_start_alt(); return true; }
// rtl_climb_start - initialise climb to RTL altitude void Copter::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // RTL_SPEED == 0 means use WPNAV_SPEED if (g.rtl_speed_cms != 0) { wp_nav.set_speed_xy(g.rtl_speed_cms); } // set the destination if (!wp_nav.set_wp_destination(rtl_path.climb_target)) { // this should not happen because rtl_build_path will have checked terrain data was available Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); return; } wp_nav.set_fast_waypoint(true); // hold current yaw during initial climb set_auto_yaw_mode(AUTO_YAW_HOLD); }
// auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips) if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); return false; } // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true; }else{ return false; } }
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z; // get final angle, 1 = Relative, 0 = Absolute if (relative_angle == 0) { // absolute angle yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed if (is_zero(turn_rate_dps)) { // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; }else{ int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec } // set yaw mode set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise }
// initialise guided mode's posvel controller void Sub::guided_posvel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_PosVel; pos_control.init_xy_controller(); // set speed and acceleration from wpnav's speed and acceleration pos_control.set_max_speed_xy(wp_nav.get_speed_xy()); pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration()); const Vector3f& curr_pos = inertial_nav.get_position(); const Vector3f& curr_vel = inertial_nav.get_velocity(); // set target position and velocity to current position and velocity pos_control.set_xy_target(curr_pos.x, curr_pos.y); pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y); // set vertical speed and acceleration pos_control.set_max_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); pos_control.set_max_accel_z(wp_nav.get_accel_z()); // pilot always controls yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }
// auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, // as this will force the helicopter to descend. if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ return false; } #endif if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true; }else{ return false; } }
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // To-Do: re-initialise wpnav targets return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { // check if heading is within 2 degrees of heading when vehicle was armed if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { rtl_state_complete = true; } } else { // we have loitered long enough rtl_state_complete = true; } } }
// verify_yaw - return true if we have reached the desired heading bool Sub::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading return (fabsf(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200); }
// rtl_return_start - initialise return to home void Copter::rtl_return_start() { rtl_state = RTL_ReturnHome; rtl_state_complete = false; wp_nav.set_wp_destination(rtl_path.return_target); // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); }
// guided_init - initialise guided controller bool Sub::guided_init(bool ignore_checks) { if (!position_ok() && !ignore_checks) { return false; } // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); // start in position control mode guided_pos_control_start(); return true; }
// guided_init - initialise guided controller bool Copter::guided_init(bool ignore_checks) { if (position_ok() || ignore_checks) { // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); // start in position control mode guided_pos_control_start(); return true; }else{ return false; } }
// auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more void Sub::auto_spline_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rat target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller wp_nav.update_spline(); float lateral_out, forward_out; translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // get pilot desired lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain()); } }
// auto_land_start - initialises controller to implement a landing void Copter::auto_land_start(const Vector3f& destination) { auto_mode = Auto_Land; // initialise loiter target destination wp_nav.init_loiter_target(destination); // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }
// rtl_return_start - initialise return to home void Copter::rtl_return_start() { rtl_state = RTL_ReturnHome; rtl_state_complete = false; if (!wp_nav.set_wp_destination(rtl_path.return_target)) { // failure must be caused by missing terrain data, restart RTL rtl_restart_without_terrain(); } // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); }
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination void Copter::auto_wp_start(const Vector3f& destination) { auto_mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) wp_nav.set_wp_destination(destination, false); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI if (auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } }
// guided_vel_control_run - runs the guided velocity controller // called from guided_run void Copter::guided_vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { // initialise velocity controller pos_control.init_vel_controller_xyz(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // set velocity to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f)); } else { guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } // call velocity controller which includes z axis controller pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } }
// guided_vel_control_run - runs the guided velocity controller // called from guided_run void Sub::guided_vel_control_run() { // ifmotors not enabled set throttle to zero and exit immediately if (!motors.armed()) { // initialise velocity controller pos_control.init_vel_controller_xyz(); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.pilot_input) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { pos_control.set_desired_velocity(Vector3f(0,0,0)); } // call velocity controller which includes z axis controller pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); float lateral_out, forward_out; translate_pos_control_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } }
// auto_land_start - initialises controller to implement a landing void Copter::auto_land_start(const Vector3f& destination) { auto_mode = Auto_Land; // initialise loiter target destination wp_nav.init_loiter_target(destination); // initialise position and desired velocity pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }
// verify_yaw - return true if we have reached the desired heading bool Copter::ModeAuto::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.yaw_look_at_heading)) <= 200) { return true; }else{ return false; } }
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks void Copter::auto_circle_movetoedge_start() { // check our distance from edge of circle Vector3f circle_edge; circle_nav.get_closest_point_on_circle(circle_edge); // set the state to move to the edge of the circle auto_mode = Auto_CircleMoveToEdge; // initialise wpnav to move to edge of circle wp_nav.set_wp_destination(circle_edge); // if we are outside the circle, point at the edge, otherwise hold yaw const Vector3f &curr_pos = inertial_nav.get_position(); const Vector3f &circle_center = circle_nav.get_center(); float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y); if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle set_auto_yaw_mode(AUTO_YAW_HOLD); } }
// rtl_descent_start - initialise descent to final alt void Copter::rtl_descent_start() { rtl_state = RTL_FinalDescent; rtl_state_complete = false; // Set wp navigation target to above home wp_nav.init_loiter_target(wp_nav.get_wp_destination()); // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more void Copter::rtl_climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // To-Do: re-initialise wpnav targets return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL rtl_state_complete = wp_nav.reached_wp_destination(); }
// auto_takeoff_start - initialises waypoint controller to implement take-off void Copter::auto_takeoff_start(float final_alt_above_home) { auto_mode = Auto_TakeOff; // initialise wpnav destination Vector3f target_pos = inertial_nav.get_position(); target_pos.z = pv_alt_above_origin(final_alt_above_home); wp_nav.set_wp_destination(target_pos); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); }
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller // seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_destination) { auto_mode = Auto_Spline; // initialise wpnav wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI if (auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } }
// rtl_loiterathome_start - initialise controllers to loiter over home void Copter::rtl_land_start() { rtl_state = RTL_Land; rtl_state_complete = false; // Set wp navigation target to above home wp_nav.init_loiter_target(wp_nav.get_wp_destination()); // initialise position and desired velocity pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); }