diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 87733d34cf..4359dfc5d9 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1,807 +1,654 @@ #include "Copter.h" -#if MODE_AUTO_ENABLED == ENABLED +#if MODE_GUIDED_ENABLED == ENABLED /* - * Init and run calls for auto flight mode - * - * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode - * Command execution code (i.e. command_logic.pde) should: - * a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called - * b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start() - * c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed - * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call - * correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature + * Init and run calls for guided flight mode */ -/* - * While in the auto flight mode, navigation or do/now commands can be run. - * Code in this file implements the navigation commands - */ - -// auto_init - initialise auto controller -bool ModeAuto::init(bool ignore_checks) -{ - if (mission.num_commands() > 1 || ignore_checks) { - _mode = SubMode::LOITER; - - // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) - if (motors->armed() && copter.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) { - auto_yaw.set_mode(AUTO_YAW_HOLD); - } - - // initialise waypoint and spline controller - wp_nav->wp_and_spline_init(); - - // set flag to start mission - waiting_to_start = true; - - // initialise mission change check (ignore results) - check_for_mission_change(); - - // clear guided limits - copter.mode_guided.limit_clear(); - - return true; - } else { - return false; - } -} +#ifndef GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM + # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away +#endif -// stop mission when we leave auto mode -void ModeAuto::exit() -{ - if (copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) { - copter.mode_auto.mission.stop(); - } -#if HAL_MOUNT_ENABLED - copter.camera_mount.set_mode_to_default(); -#endif // HAL_MOUNT_ENABLED +static Vector3p guided_pos_target_cm; // position target (used by posvel controller only) +bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain +static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller) +static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller) +static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller + +struct { + uint32_t update_time_ms; + float roll_cd; + float pitch_cd; + float yaw_cd; + float yaw_rate_cds; + float climb_rate_cms; // climb rate in cms. Used if use_thrust is false + float thrust; // thrust from -1 to 1. Used if use_thrust is true + bool use_yaw_rate; + bool use_thrust; +} static guided_angle_state; + +struct Guided_Limit { + uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked + float alt_min_cm; // lower altitude limit in cm above home (0 = no limit) + float alt_max_cm; // upper altitude limit in cm above home (0 = no limit) + float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit) + uint32_t start_time;// system time in milliseconds that control was handed to the external computer + Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit +} guided_limit; + +// init - initialise guided controller +bool ModeGuided::init(bool ignore_checks) +{ + // start in velaccel control mode + velaccel_control_start(); + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + send_notification = false; + return true; } -// auto_run - runs the auto controller -// should be called at 100hz or more -void ModeAuto::run() +// run - runs the guided controller +// should be called at 100hz or more +void ModeGuided::run() { - // start or update mission - if (waiting_to_start) { - // don't start the mission until we have an origin - Location loc; - if (copter.ahrs.get_origin(loc)) { - // start/resume the mission (based on MIS_RESTART parameter) - mission.start_or_resume(); - waiting_to_start = false; - - // initialise mission change check (ignore results) - check_for_mission_change(); - } - } else { - // check for mission changes - if (check_for_mission_change()) { - // if mission is running restart the current command if it is a waypoint or spline command - if ((copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == SubMode::WP)) { - if (mission.restart_current_nav_cmd()) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); - } else { - // failed to restart mission for some reason - gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command"); - } - } - } - - mission.update(); - } - // call the correct auto controller - switch (_mode) { + switch (guided_mode) { - case SubMode::TAKEOFF: + case SubMode::TakeOff: + // run takeoff controller takeoff_run(); break; case SubMode::WP: - case SubMode::CIRCLE_MOVE_TO_EDGE: - wp_run(); - break; - - case SubMode::LAND: - land_run(); - break; - - case SubMode::RTL: - rtl_run(); + // run waypoint controller + wp_control_run(); + if (send_notification && wp_nav->reached_wp_destination()) { + send_notification = false; + gcs().send_mission_item_reached_message(0); + } break; - case SubMode::CIRCLE: - circle_run(); + case SubMode::Pos: + // run position controller + pos_control_run(); break; - case SubMode::NAVGUIDED: -#if NAV_GUIDED == ENABLED - nav_guided_run(); -#endif + case SubMode::Accel: + accel_control_run(); break; - case SubMode::LOITER: - loiter_run(); + case SubMode::VelAccel: + velaccel_control_run(); break; - case SubMode::LOITER_TO_ALT: - loiter_to_alt_run(); + case SubMode::PosVelAccel: + posvelaccel_control_run(); break; - case SubMode::NAV_PAYLOAD_PLACE: - payload_place_run(); + case SubMode::Angle: + angle_control_run(); break; } -} + } -bool ModeAuto::allows_arming(AP_Arming::Method method) const +bool ModeGuided::allows_arming(AP_Arming::Method method) const { - return (copter.g2.auto_options & (uint32_t)Options::AllowArming) != 0; + // always allow arming from the ground station + if (method == AP_Arming::Method::MAVLINK) { + return true; + } + + // optionally allow arming from the transmitter + return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0; }; -// auto_loiter_start - initialises loitering in auto mode -// returns success/failure because this can be called by exit_mission -bool ModeAuto::loiter_start() +// do_user_takeoff_start - initialises waypoint controller to implement take-off +bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) { - // return failure if GPS is bad - if (!copter.position_ok()) { + guided_mode = SubMode::TakeOff; + + // initialise wpnav destination + Location target_loc = copter.current_loc; + Location::AltFrame frame = Location::AltFrame::ABOVE_HOME; + if (wp_nav->rangefinder_used_and_healthy() && + wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER && + takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { + // can't takeoff downwards + if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) { + return false; + } + frame = Location::AltFrame::ABOVE_TERRAIN; + } + target_loc.set_alt_cm(takeoff_alt_cm, frame); + + if (!wp_nav->set_wp_destination_loc(target_loc)) { + // failure to set destination can only be because of missing terrain data + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + // failure is propagated to GCS with NAK return false; } - _mode = SubMode::LOITER; - // calculate stopping point - Vector3f stopping_point; - wp_nav->get_wp_stopping_point(stopping_point); + // initialise yaw + auto_yaw.set_mode(AUTO_YAW_HOLD); + + // clear i term when we're taking off + set_throttle_takeoff(); - // initialise waypoint controller target to stopping point - wp_nav->set_wp_destination(stopping_point); + // get initial alt for WP_NAVALT_MIN + auto_takeoff_set_start_alt(); - // hold yaw at current heading - auto_yaw.set_mode(AUTO_YAW_HOLD); + // record takeoff has not completed + takeoff_complete = false; return true; } -// auto_rtl_start - initialises RTL in AUTO flight mode -void ModeAuto::rtl_start() +// initialise guided mode's waypoint navigation controller +void ModeGuided::wp_control_start() { - _mode = SubMode::RTL; + // set to position control mode + guided_mode = SubMode::WP; - // call regular rtl flight mode initialisation and ask it to ignore checks - copter.mode_rtl.init(true); + // initialise waypoint and spline controller + wp_nav->wp_and_spline_init(); + + // initialise wpnav to stopping point + Vector3f stopping_point; + wp_nav->get_wp_stopping_point(stopping_point); + + // no need to check return status because terrain data is not used + wp_nav->set_wp_destination(stopping_point, false); + + // initialise yaw + auto_yaw.set_mode_to_default(false); } -// auto_takeoff_start - initialises waypoint controller to implement take-off -void ModeAuto::takeoff_start(const Location& dest_loc) +// run guided mode's waypoint navigation controller +void ModeGuided::wp_control_run() { - _mode = SubMode::TAKEOFF; - - Location dest(dest_loc); + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + // 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)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } + } - if (!copter.current_loc.initialised()) { - // vehicle doesn't know where it is ATM. We should not - // initialise our takeoff destination without knowing this! + // if not armed set throttle to zero and exit immediately + if (is_disarmed_or_landed()) { + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } - // set horizontal target - dest.lat = copter.current_loc.lat; - dest.lng = copter.current_loc.lng; + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // get altitude target - int32_t alt_target; - if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) { - // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); - // fall back to altitude above current altitude - alt_target = copter.current_loc.alt + dest.alt; - } + // run waypoint controller + copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - // sanity check target - int32_t alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0); - if (alt_target < alt_target_min_cm ) { - dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME); - } + // call z-axis position controller (wpnav should have already updated it's alt target) + pos_control->update_z_controller(); - // set waypoint controller target - if (!wp_nav->set_wp_destination_loc(dest)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); - return; + // call attitude controller + if (auto_yaw.mode() == AUTO_YAW_HOLD) { + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds()); + } else { + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } +} + +// initialise position controller +void ModeGuided::pva_control_start() +{ + // initialise horizontal speed, acceleration + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialize vertical speeds and acceleration + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + + // initialise velocity controller + pos_control->init_z_controller(); + pos_control->init_xy_controller(); // initialise yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode_to_default(false); - // clear i term when we're taking off - set_throttle_takeoff(); + // initialise terrain alt + guided_pos_terrain_alt = false; +} - // get initial alt for WP_NAVALT_MIN - auto_takeoff_set_start_alt(); +// initialise guided mode's position controller +void ModeGuided::pos_control_start() +{ + // set to position control mode + guided_mode = SubMode::Pos; + + // initialise position controller + pva_control_start(); } -// auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void ModeAuto::wp_start(const Location& dest_loc) +// initialise guided mode's velocity controller +void ModeGuided::accel_control_start() { - // send target to waypoint controller - if (!wp_nav->set_wp_destination_loc(dest_loc)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); - return; - } + // set guided_mode to velocity controller + guided_mode = SubMode::Accel; + + // initialise position controller + pva_control_start(); +} - _mode = SubMode::WP; +// initialise guided mode's velocity and acceleration controller +void ModeGuided::velaccel_control_start() +{ + // set guided_mode to velocity controller + guided_mode = SubMode::VelAccel; - // 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) { - auto_yaw.set_mode_to_default(false); - } + // initialise position controller + pva_control_start(); } -// auto_land_start - initialises controller to implement a landing -void ModeAuto::land_start() +// initialise guided mode's position, velocity and acceleration controller +void ModeGuided::posvelaccel_control_start() { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); + // set guided_mode to velocity controller + guided_mode = SubMode::PosVelAccel; - // call location specific land start function - land_start(stopping_point); + // initialise position controller + pva_control_start(); } -// auto_land_start - initialises controller to implement a landing -void ModeAuto::land_start(const Vector2f& destination) +bool ModeGuided::is_taking_off() const { - _mode = SubMode::LAND; + return guided_mode == SubMode::TakeOff && !takeoff_complete; +} + +// initialise guided mode's angle controller +void ModeGuided::angle_control_start() +{ + // set guided_mode to velocity controller + guided_mode = SubMode::Angle; - // initialise loiter target destination - loiter_nav->init_target(destination); + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise the vertical position controller if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } - // initialise yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); - -#if LANDING_GEAR_ENABLED == ENABLED - // optionally deploy landing gear - copter.landinggear.deploy_for_landing(); -#endif + // 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; -#if AC_FENCE == ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif + // pilot always controls yaw + auto_yaw.set_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 ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m) +// 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 ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt) { - // set circle center - copter.circle_nav->set_center(circle_center); - - // set circle radius - if (!is_zero(radius_m)) { - copter.circle_nav->set_radius(radius_m * 100.0f); +#if AC_FENCE == ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + if (!copter.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; } +#endif - // check our distance from edge of circle - Vector3f circle_edge_neu; - copter.circle_nav->get_closest_point_on_circle(circle_edge_neu); - float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length(); + // if configured to use wpnav for position control + if (use_wpnav_for_position_control()) { + // ensure we are in position control mode + if (guided_mode != SubMode::WP) { + wp_control_start(); + } - // 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 - _mode = SubMode::CIRCLE_MOVE_TO_EDGE; + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - // convert circle_edge_neu to Location - Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); + // no need to check return status because terrain data is not used + wp_nav->set_wp_destination(destination, terrain_alt); - // convert altitude to same as command - circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); + // log target + copter.Log_Write_GuidedTarget(guided_mode, destination, terrain_alt, Vector3f(), Vector3f()); + send_notification = true; + return true; + } - // initialise wpnav to move to edge of circle - if (!wp_nav->set_wp_destination_loc(circle_edge)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); - } + // if configured to use position controller for position control + // ensure we are in position control mode + if (guided_mode != SubMode::Pos) { + pos_control_start(); + } - // if we are outside the circle, point at the edge, otherwise hold yaw - const Vector3p &circle_center_neu = copter.circle_nav->get_center(); - 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); - // 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) { - if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) { - auto_yaw.set_mode_to_default(false); - } else { - // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle - auto_yaw.set_mode(AUTO_YAW_HOLD); - } + // initialise terrain following if needed + if (terrain_alt) { + // get current alt above terrain + float origin_terr_offset; + if (!wp_nav->get_terrain_offset(origin_terr_offset)) { + // if we don't have terrain altitude then stop + init(true); + return false; + } + // convert origin to alt-above-terrain if necessary + if (!guided_pos_terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + pos_control->set_pos_offset_z_cm(origin_terr_offset); } } else { - circle_start(); + pos_control->set_pos_offset_z_cm(0.0); } -} - -// auto_circle_start - initialises controller to fly a circle in AUTO flight mode -// assumes that circle_nav object has already been initialised with circle center and radius -void ModeAuto::circle_start() -{ - _mode = SubMode::CIRCLE; - // initialise circle controller - copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt()); + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - if (auto_yaw.mode() != AUTO_YAW_ROI) { - auto_yaw.set_mode(AUTO_YAW_CIRCLE); - } -} + // set position target and zero velocity and acceleration + guided_pos_target_cm = destination.topostype(); + guided_pos_terrain_alt = terrain_alt; + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + update_time_ms = millis(); -#if NAV_GUIDED == ENABLED -// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void ModeAuto::nav_guided_start() -{ - _mode = SubMode::NAVGUIDED; + // log target + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); - // call regular guided flight mode initialisation - copter.mode_guided.init(true); + send_notification = true; - // initialise guided start time and position as reference for limit checking - copter.mode_guided.limit_init_time_and_pos(); + return true; } -#endif //NAV_GUIDED -bool ModeAuto::is_landing() const +bool ModeGuided::get_wp(Location& destination) { - switch(_mode) { - case SubMode::LAND: + switch (guided_mode) { + case SubMode::WP: + return wp_nav->get_oa_wp_destination(destination); + case SubMode::Pos: + destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); return true; - case SubMode::RTL: - return copter.mode_rtl.is_landing(); default: return false; } - return false; -} - -bool ModeAuto::is_taking_off() const -{ - return ((_mode == SubMode::TAKEOFF) && !wp_nav->reached_wp_destination()); -} - -// auto_payload_place_start - initialises controller to implement a placing -void ModeAuto::payload_place_start() -{ - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - - // call location specific place start function - payload_place_start(stopping_point); -} -// returns true if pilot's yaw input should be used to adjust vehicle's heading -bool ModeAuto::use_pilot_yaw(void) const -{ - return (copter.g2.auto_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0; + // should never get here but just in case + return false; } -// start_command - this function will be called when the ap_mission lib wishes to start a new command -bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) +// sets guided mode's target from a Location object +// returns false if destination could not be set (probably caused by missing terrain data) +// or if the fence is enabled and guided waypoint is outside the fence +bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { - // To-Do: logging when new commands start/end - if (copter.should_log(MASK_LOG_CMD)) { - copter.logger.Write_Mission_Cmd(mission, cmd); +#if AC_FENCE == ENABLED + // reject destination outside the fence. + // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails + if (!copter.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; } - - switch(cmd.id) { - - /// - /// navigation commands - /// - case MAV_CMD_NAV_TAKEOFF: // 22 - do_takeoff(cmd); - break; - - case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint - do_nav_wp(cmd); - break; - - case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint - do_land(cmd); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely - do_loiter_unlimited(cmd); - break; - - case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times - do_circle(cmd); - break; - - case MAV_CMD_NAV_LOITER_TIME: // 19 - do_loiter_time(cmd); - break; - - case MAV_CMD_NAV_LOITER_TO_ALT: - do_loiter_to_alt(cmd); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20 - do_RTL(); - break; - - case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline - do_spline_wp(cmd); - break; - -#if NAV_GUIDED == ENABLED - case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer - do_nav_guided_enable(cmd); - break; #endif - case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command - do_nav_delay(cmd); - break; - - case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint - do_payload_place(cmd); - break; + // if using wpnav for position control + if (use_wpnav_for_position_control()) { + if (guided_mode != SubMode::WP) { + wp_control_start(); + } - // - // conditional commands - // - case MAV_CMD_CONDITION_DELAY: // 112 - do_wait_delay(cmd); - break; + if (!wp_nav->set_wp_destination_loc(dest_loc)) { + // failure to set destination can only be because of missing terrain data + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + // failure is propagated to GCS with NAK + return false; + } - case MAV_CMD_CONDITION_DISTANCE: // 114 - do_within_distance(cmd); - break; + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - case MAV_CMD_CONDITION_YAW: // 115 - do_yaw(cmd); - break; + // log target + copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f()); + send_notification = true; + return true; + } - /// - /// do commands - /// - case MAV_CMD_DO_CHANGE_SPEED: // 178 - do_change_speed(cmd); - break; + // if configured to use position controller for position control + // ensure we are in position control mode + if (guided_mode != SubMode::Pos) { + pos_control_start(); + } - case MAV_CMD_DO_SET_HOME: // 179 - do_set_home(cmd); - break; + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - case MAV_CMD_DO_SET_ROI: // 201 - // point the copter and camera at a region of interest (ROI) - do_roi(cmd); - break; + // set position target and zero velocity and acceleration + Vector3f pos_target_f; + bool terrain_alt; + if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) { + return false; + } - case MAV_CMD_DO_MOUNT_CONTROL: // 205 - // point the camera to a specified angle - do_mount_control(cmd); - break; - - case MAV_CMD_DO_FENCE_ENABLE: -#if AC_FENCE == ENABLED - if (cmd.p1 == 0) { //disable - copter.fence.enable(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - copter.fence.enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); + // initialise terrain following if needed + if (terrain_alt) { + // get current alt above terrain + float origin_terr_offset; + if (!wp_nav->get_terrain_offset(origin_terr_offset)) { + // if we don't have terrain altitude then stop + init(true); + return false; } -#endif //AC_FENCE == ENABLED - break; + // convert origin to alt-above-terrain if necessary + if (!guided_pos_terrain_alt) { + // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin + pos_control->set_pos_offset_z_cm(origin_terr_offset); + } + } else { + pos_control->set_pos_offset_z_cm(0.0); + } -#if NAV_GUIDED == ENABLED - case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits - do_guided_limits(cmd); - break; -#endif + guided_pos_target_cm = pos_target_f.topostype(); + guided_pos_terrain_alt = terrain_alt; + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); -#if WINCH_ENABLED == ENABLED - case MAV_CMD_DO_WINCH: // Mission command to control winch - do_winch(cmd); - break; -#endif + // log target + copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); - default: - // unable to use the command, allow the vehicle to try the next command - return false; - } + send_notification = true; - // always return success return true; } -// exit_mission - function that is called once the mission completes -void ModeAuto::exit_mission() +// set_velaccel - sets guided mode's target velocity and acceleration +void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { - // play a tone - AP_Notify::events.mission_complete = 1; - // if we are not on the ground switch to loiter or land - if (!copter.ap.land_complete) { - // try to enter loiter but if that fails land - if (!loiter_start()) { - set_mode(Mode::Number::LAND, ModeReason::MISSION_END); - } - } else { - // if we've landed it's safe to disarm - copter.arming.disarm(AP_Arming::Method::MISSIONEXIT); + // check we are in velocity control mode + if (guided_mode != SubMode::Accel) { + accel_control_start(); } -} -// detect external changes to mission -bool ModeAuto::check_for_mission_change() -{ - // check if mission has been updated - const uint32_t change_time_ms = mission.last_change_time_ms(); - const bool update_time_changed = (change_time_ms != mis_change_detect.last_change_time_ms);; + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - // check if active command index has changed - const uint16_t curr_cmd_idx = mission.get_current_nav_index(); - const bool curr_cmd_idx_changed = (curr_cmd_idx != mis_change_detect.curr_cmd_index); + // set velocity and acceleration targets and zero position + guided_pos_target_cm.zero(); + guided_pos_terrain_alt = false; + guided_vel_target_cms.zero(); + guided_accel_target_cmss = acceleration; + update_time_ms = millis(); - // no changes if neither mission update time nor active command index has changed - if (!update_time_changed && !curr_cmd_idx_changed) { - return false; + // log target + if (log_request) { + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } +} - // the mission has been updated (but maybe not changed) and/or the current command index has changed - // check the contents of the next three commands to ensure they have not changed - // and update storage so we can detect future changes - - bool cmds_changed = false; // true if upcoming command contents have changed +// set_velocity - sets guided mode's target velocity +void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +{ + set_velaccel(velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw, log_request); +} - // retrieve cmds from mission and compare with mis_change_detect - uint8_t num_cmds = 0; - uint16_t cmd_idx = curr_cmd_idx; - AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; - while ((num_cmds < ARRAY_SIZE(cmd)) && mission.get_next_nav_cmd(cmd_idx, cmd[num_cmds])) { - num_cmds++; - if ((num_cmds > mis_change_detect.cmd_count) || (cmd[num_cmds-1] != mis_change_detect.cmd[num_cmds-1])) { - cmds_changed = true; - mis_change_detect.cmd[num_cmds-1] = cmd[num_cmds-1]; - } - cmd_idx = cmd[num_cmds-1].index+1; +// set_velaccel - sets guided mode's target velocity and acceleration +void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +{ + // check we are in velocity control mode + if (guided_mode != SubMode::VelAccel) { + velaccel_control_start(); } - // mission has changed if number of upcoming commands does not match mis_change_detect - if (num_cmds != mis_change_detect.cmd_count) { - cmds_changed = true; - } + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + + // set velocity and acceleration targets and zero position + guided_pos_target_cm.zero(); + guided_pos_terrain_alt = false; + guided_vel_target_cms = velocity; + guided_accel_target_cmss = acceleration; + update_time_ms = millis(); - // update mis_change_detect with last change time, command index and number of commands - mis_change_detect.last_change_time_ms = change_time_ms; - mis_change_detect.curr_cmd_index = curr_cmd_idx; - mis_change_detect.cmd_count = num_cmds; + // log target + if (log_request) { + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); + } +} - // mission has changed if upcoming command contents have changed without the current command index changing - return cmds_changed && !curr_cmd_idx_changed; +// set_destination_posvel - set guided mode position and velocity target +bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ + return set_destination_posvelaccel(destination, velocity, Vector3f(), use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); } -// do_guided - start guided mode -bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) +// set_destination_posvelaccel - set guided mode position, velocity and acceleration target +bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { - // only process guided waypoint if we are in guided mode - if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && mode() == SubMode::NAVGUIDED)) { +#if AC_FENCE == ENABLED + // reject destination if outside the fence + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + if (!copter.fence.check_destination_within_fence(dest_loc)) { + AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK return false; } +#endif - // switch to handle different commands - switch (cmd.id) { - - case MAV_CMD_NAV_WAYPOINT: - { - // set wp_nav's destination - Location dest(cmd.content.location); - return copter.mode_guided.set_destination(dest); - } + // check we are in velocity control mode + if (guided_mode != SubMode::PosVelAccel) { + posvelaccel_control_start(); + } - case MAV_CMD_CONDITION_YAW: - do_yaw(cmd); - return true; + // set yaw state + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); - default: - // reject unrecognised command - return false; - } + update_time_ms = millis(); + guided_pos_target_cm = destination.topostype(); + guided_pos_terrain_alt = false; + guided_vel_target_cms = velocity; + guided_accel_target_cmss = acceleration; + // log target + copter.Log_Write_GuidedTarget(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); return true; } -uint32_t ModeAuto::wp_distance() const +// returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate +bool ModeGuided::set_attitude_target_provides_thrust() const { - switch (_mode) { - case SubMode::CIRCLE: - return copter.circle_nav->get_distance_to_target(); - case SubMode::WP: - case SubMode::CIRCLE_MOVE_TO_EDGE: - default: - return wp_nav->get_wp_distance_to_destination(); - } + return ((copter.g2.guided_options.get() & uint32_t(Options::SetAttitudeTarget_ThrustAsThrust)) != 0); } -int32_t ModeAuto::wp_bearing() const +// returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active) +bool ModeGuided::stabilizing_pos_xy() const { - switch (_mode) { - case SubMode::CIRCLE: - return copter.circle_nav->get_bearing_to_target(); - case SubMode::WP: - case SubMode::CIRCLE_MOVE_TO_EDGE: - default: - return wp_nav->get_wp_bearing_to_destination(); - } + return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizePositionXY)) != 0); } -bool ModeAuto::get_wp(Location& destination) +// returns true if GUIDED_OPTIONS param specifies velocity should be controlled (when acceleration control is active) +bool ModeGuided::stabilizing_vel_xy() const { - switch (_mode) { - case SubMode::NAVGUIDED: - return copter.mode_guided.get_wp(destination); - case SubMode::WP: - return wp_nav->get_oa_wp_destination(destination); - case SubMode::RTL: - return copter.mode_rtl.get_wp(destination); - default: - return false; - } + return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0); } -/******************************************************************************* -Verify command Handlers - -Each type of mission element has a "verify" operation. The verify -operation returns true when the mission element has completed and we -should move onto the next mission element. -Return true if we do not recognize the command so that we move on to the next command -*******************************************************************************/ +// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower) +bool ModeGuided::use_wpnav_for_position_control() const +{ + return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0); +} -// verify_command - callback function called from ap-mission at 10hz or higher when a command is being run -// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode -bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) +// set guided mode angle target and climbrate +void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust) { - if (copter.flightmode != &copter.mode_auto) { - return false; + // check we are in velocity control mode + if (guided_mode != SubMode::Angle) { + angle_control_start(); } - bool cmd_complete = false; - - switch (cmd.id) { - // - // navigation commands - // - case MAV_CMD_NAV_TAKEOFF: - cmd_complete = verify_takeoff(); - break; + // convert quaternion to euler angles + q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); + guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; + guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; + guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); + guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f; + guided_angle_state.use_yaw_rate = use_yaw_rate; - case MAV_CMD_NAV_WAYPOINT: - cmd_complete = verify_nav_wp(cmd); - break; - - case MAV_CMD_NAV_LAND: - cmd_complete = verify_land(); - break; - - case MAV_CMD_NAV_PAYLOAD_PLACE: - cmd_complete = verify_payload_place(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: - cmd_complete = verify_loiter_unlimited(); - break; - - case MAV_CMD_NAV_LOITER_TURNS: - cmd_complete = verify_circle(cmd); - break; - - case MAV_CMD_NAV_LOITER_TIME: - cmd_complete = verify_loiter_time(cmd); - break; - - case MAV_CMD_NAV_LOITER_TO_ALT: - return verify_loiter_to_alt(); - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - cmd_complete = verify_RTL(); - break; - - case MAV_CMD_NAV_SPLINE_WAYPOINT: - cmd_complete = verify_spline_wp(cmd); - break; - -#if NAV_GUIDED == ENABLED - case MAV_CMD_NAV_GUIDED_ENABLE: - cmd_complete = verify_nav_guided_enable(cmd); - break; -#endif - - case MAV_CMD_NAV_DELAY: - cmd_complete = verify_nav_delay(cmd); - break; - - /// - /// conditional commands - /// - case MAV_CMD_CONDITION_DELAY: - cmd_complete = verify_wait_delay(); - break; - - case MAV_CMD_CONDITION_DISTANCE: - cmd_complete = verify_within_distance(); - break; - - case MAV_CMD_CONDITION_YAW: - cmd_complete = verify_yaw(); - break; - - // do commands (always return true) - case MAV_CMD_DO_CHANGE_SPEED: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_ROI: - case MAV_CMD_DO_MOUNT_CONTROL: - case MAV_CMD_DO_GUIDED_LIMITS: - case MAV_CMD_DO_FENCE_ENABLE: - case MAV_CMD_DO_WINCH: - cmd_complete = true; - break; - - default: - // error message - gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id); - // return true if we do not recognize the command so that we move on to the next command - cmd_complete = true; - break; + guided_angle_state.use_thrust = use_thrust; + if (use_thrust) { + guided_angle_state.thrust = climb_rate_cms_or_thrust; + guided_angle_state.climb_rate_cms = 0.0f; + } else { + guided_angle_state.thrust = 0.0f; + guided_angle_state.climb_rate_cms = climb_rate_cms_or_thrust; } + guided_angle_state.update_time_ms = millis(); - // send message to GCS - if (cmd_complete) { - gcs().send_mission_item_reached_message(cmd.index); - } - - return cmd_complete; + // log target + copter.Log_Write_GuidedTarget(guided_mode, + Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), + false, + Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f()); } -// takeoff_run - takeoff in auto mode -// called by auto_run at 100hz or more -void ModeAuto::takeoff_run() +// takeoff_run - takeoff in guided mode +// called by guided_run at 100hz or more +void ModeGuided::takeoff_run() { - // if the user doesn't want to raise the throttle we can set it automatically - // note that this can defeat the disarm check on takeoff - if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) { - copter.set_auto_armed(true); - } auto_takeoff_run(); + if (!takeoff_complete && wp_nav->reached_wp_destination()) { + takeoff_complete = true; +#if LANDING_GEAR_ENABLED == ENABLED + // optionally retract landing gear + copter.landinggear.retract_after_takeoff(); +#endif + } } -// auto_wp_run - runs the auto waypoint controller -// called by auto_run at 100hz or more -void ModeAuto::wp_run() +// pos_control_run - runs the guided position controller +// called from guided_run +void ModeGuided::pos_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { @@ -811,1153 +658,515 @@ void ModeAuto::wp_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_ground_handling(); - wp_nav->wp_and_spline_init(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); + return; + } + + // calculate terrain adjustments + float terr_offset = 0.0f; + if (guided_pos_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { + // failure to set destination can only be because of missing terrain data + copter.failsafe_terrain_on_event(); return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // run waypoint controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + // send position and velocity targets to position controller + guided_accel_target_cmss.zero(); + guided_vel_target_cms.zero(); - // WP_Nav has set the vertical position control targets - // run the vertical position controller and set output throttle + float pos_offset_z_buffer = 0.0; // Vertical buffer size in m + if (guided_pos_terrain_alt) { + pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); + } + pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); + + // run position controllers + pos_control->update_xy_controller(); 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_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } -// auto_land_run - lands in auto mode -// called by auto_run at 100hz or more -void ModeAuto::land_run() +// velaccel_control_run - runs the guided velocity controller +// called from guided_run +void ModeGuided::accel_control_run() { + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + // 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)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } + } // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - land_run_horizontal_control(); - land_run_vertical_control(); -} -// auto_rtl_run - rtl in AUTO flight mode -// called by auto_run at 100hz or more -void ModeAuto::rtl_run() -{ - // call regular rtl flight mode run function - copter.mode_rtl.run(false); -} - -// auto_circle_run - circle in AUTO flight mode -// called by auto_run at 100hz or more -void ModeAuto::circle_run() -{ - // process pilot's yaw input - float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { - // 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)) { - auto_yaw.set_mode(AUTO_YAW_HOLD); + // set velocity to zero and stop rotating if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - update_time_ms > get_timeout_ms()) { + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { + auto_yaw.set_rate(0.0f); + } + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false); + } else { + // update position controller with new target + pos_control->input_accel_xy(guided_accel_target_cmss); + if (!stabilizing_vel_xy()) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy()) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); } + pos_control->input_accel_z(guided_accel_target_cmss.z); } - // call circle controller - copter.failsafe_terrain_set_status(copter.circle_nav->update()); - - // WP_Nav has set the vertical position control targets - // run the vertical position controller and set output throttle + // call velocity controller which includes z axis controller + pos_control->update_xy_controller(); 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_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate); + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), auto_yaw.yaw()); + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } -#if NAV_GUIDED == ENABLED -// auto_nav_guided_run - allows control by external navigation controller -// called by auto_run at 100hz or more -void ModeAuto::nav_guided_run() +// velaccel_control_run - runs the guided velocity and acceleration controller +// called from guided_run +void ModeGuided::velaccel_control_run() { - // call regular guided flight mode run function - copter.mode_guided.run(); -} -#endif // NAV_GUIDED + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + // 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)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } + } -// auto_loiter_run - loiter in AUTO flight mode -// called by auto_run at 100hz or more -void ModeAuto::loiter_run() -{ // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { - make_safe_ground_handling(); - wp_nav->wp_and_spline_init(); + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } - // accept pilot input of yaw - float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - } - // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // run waypoint and z-axis position controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - - pos_control->update_z_controller(); - attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); -} - -// auto_loiter_run - loiter to altitude in AUTO flight mode -// called by auto_run at 100hz or more -void ModeAuto::loiter_to_alt_run() -{ - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (is_disarmed_or_landed() || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - return; - } - - // possibly just run the waypoint controller: - if (!loiter_to_alt.reached_destination_xy) { - loiter_to_alt.reached_destination_xy = wp_nav->reached_wp_destination_xy(); - if (!loiter_to_alt.reached_destination_xy) { - wp_run(); - return; + // set velocity to zero and stop rotating if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - update_time_ms > get_timeout_ms()) { + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { + auto_yaw.set_rate(0.0f); } } - if (!loiter_to_alt.loiter_start_done) { - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); - _mode = SubMode::LOITER_TO_ALT; - loiter_to_alt.loiter_start_done = true; - } - const float alt_error_cm = copter.current_loc.alt - loiter_to_alt.alt; - if (fabsf(alt_error_cm) < 5.0) { // random numbers R US - loiter_to_alt.reached_alt = true; - } else if (alt_error_cm * loiter_to_alt.alt_error_cm < 0) { - // we were above and are now below, or vice-versa - loiter_to_alt.reached_alt = true; - } - loiter_to_alt.alt_error_cm = alt_error_cm; - - // loiter... - - land_run_horizontal_control(); - - // Compute a vertical velocity demand such that the vehicle - // approaches the desired altitude. - float target_climb_rate = sqrt_controller( - -alt_error_cm, - pos_control->get_pos_z_p().kP(), - pos_control->get_max_accel_z_cmss(), - G_Dt); - target_climb_rate = constrain_float(target_climb_rate, pos_control->get_max_speed_down_cms(), pos_control->get_max_speed_up_cms()); - - // get avoidance adjusted climb rate - target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - - // update the vertical offset based on the surface measurement - copter.surface_tracking.update_surface_offset(); - - // Send the commanded climb rate to the position controller - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); - - pos_control->update_z_controller(); -} - -// auto_payload_place_start - initialises controller to implement placement of a load -void ModeAuto::payload_place_start(const Vector2f& destination) -{ - _mode = SubMode::NAV_PAYLOAD_PLACE; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - - // initialise loiter target destination - loiter_nav->init_target(destination); - - // initialise the vertical position controller - pos_control->init_z_controller(); - - // initialise yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); -} - -// auto_payload_place_run - places an object in auto mode -// called by auto_run at 100hz or more -void ModeAuto::payload_place_run() -{ - if (!payload_place_run_should_run()) { - zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); - return; - } - - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + bool do_avoid = false; +#if AC_AVOID_ENABLED + // limit the velocity for obstacle/fence avoidance + copter.avoid.adjust_velocity(guided_vel_target_cms, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); + do_avoid = copter.avoid.limits_active(); +#endif - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - return wp_run(); - case PayloadPlaceStateType_Calibrating_Hover_Start: - case PayloadPlaceStateType_Calibrating_Hover: - return payload_place_run_loiter(); - case PayloadPlaceStateType_Descending_Start: - case PayloadPlaceStateType_Descending: - return payload_place_run_descend(); - case PayloadPlaceStateType_Releasing_Start: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Released: - case PayloadPlaceStateType_Ascending_Start: - return payload_place_run_loiter(); - case PayloadPlaceStateType_Ascending: - case PayloadPlaceStateType_Done: - return wp_run(); - } -} + // update position controller with new target -bool ModeAuto::payload_place_run_should_run() -{ - // must be armed - if (!motors->armed()) { - return false; + if (!stabilizing_vel_xy() && !do_avoid) { + // set the current commanded xy vel to the desired vel + guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; + guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; } - // must be auto-armed - if (!copter.ap.auto_armed) { - return false; - } - // must not be landed - if (copter.ap.land_complete) { - return false; + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); + if (!stabilizing_vel_xy() && !do_avoid) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy() && !do_avoid) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); } - // interlock must be enabled (i.e. unsafe) - if (!motors->get_interlock()) { - return false; - } - - return true; -} - -void ModeAuto::payload_place_run_loiter() -{ - // loiter... - land_run_horizontal_control(); + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false); - // call position controller + // call velocity controller which includes z axis controller + pos_control->update_xy_controller(); pos_control->update_z_controller(); -} -void ModeAuto::payload_place_run_descend() -{ - land_run_horizontal_control(); - land_run_vertical_control(); -} - -// terrain_adjusted_location: returns a Location with lat/lon from cmd -// and altitude from our current altitude adjusted for location -Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const -{ - // convert to location class - Location target_loc(cmd.content.location); - - // decide if we will use terrain following - int32_t curr_terr_alt_cm, target_terr_alt_cm; - if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, curr_terr_alt_cm) && - target_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, target_terr_alt_cm)) { - curr_terr_alt_cm = MAX(curr_terr_alt_cm,200); - // if using terrain, set target altitude to current altitude above terrain - target_loc.set_alt_cm(curr_terr_alt_cm, Location::AltFrame::ABOVE_TERRAIN); + // call attitude controller + if (auto_yaw.mode() == AUTO_YAW_HOLD) { + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // set target altitude to current altitude above home - target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } - return target_loc; } -/********************************************************************************/ -// Nav (Must) commands -/********************************************************************************/ - -// do_takeoff - initiate takeoff navigation command -void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd) +// posvelaccel_control_run - runs the guided position, velocity and acceleration controller +// called from guided_run +void ModeGuided::posvelaccel_control_run() { - // Set wp navigation target to safe altitude above current position - takeoff_start(cmd.content.location); -} - -Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const -{ - Location ret(cmd.content.location); - - // use current lat, lon if zero - if (ret.lat == 0 && ret.lng == 0) { - ret.lat = default_loc.lat; - ret.lng = default_loc.lng; - } - // use default altitude if not provided in cmd - if (ret.alt == 0) { - // set to default_loc's altitude but in command's alt frame - // note that this may use the terrain database - int32_t default_alt; - if (default_loc.get_alt_cm(ret.get_alt_frame(), default_alt)) { - ret.set_alt_cm(default_alt, ret.get_alt_frame()); - } else { - // default to default_loc's altitude and frame - ret.set_alt_cm(default_loc.alt, default_loc.get_alt_frame()); - } - } - return ret; -} + // process pilot's yaw input + float target_yaw_rate = 0; -// do_nav_wp - initiate move to next waypoint -void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) -{ - // calculate default location used when lat, lon or alt is zero - Location default_loc = copter.current_loc; - if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { - if (!wp_nav->get_wp_destination_loc(default_loc)) { - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 + // 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)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); } } - // get waypoint's location from command and send to wp_nav - const Location dest_loc = loc_from_cmd(cmd, default_loc); - if (!wp_nav->set_wp_destination_loc(dest_loc)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); - return; - } - - _mode = SubMode::WP; - - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; - // this is the delay, stored in seconds - loiter_time_max = cmd.p1; - - // set next destination if necessary - if (!set_next_wp(cmd, dest_loc)) { - // failure to set next destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); + // if not armed set throttle to zero and exit immediately + if (is_disarmed_or_landed()) { + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } - // 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) { - auto_yaw.set_mode_to_default(false); - } -} - -// checks the next mission command and adds it as a destination if necessary -// supports both straight line and spline waypoints -// cmd should be the current command -// default_loc should be the destination from the current_cmd but corrected for cases where user set lat, lon or alt to zero -// returns true on success, false on failure which should only happen due to a failure to retrieve terrain data -bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc) -{ - // do not add next wp if current command has a delay meaning the vehicle will stop at the destination - if (current_cmd.p1 > 0) { - return true; - } + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // do not add next wp if there are no more navigation commands - AP_Mission::Mission_Command next_cmd; - if (!mission.get_next_nav_cmd(current_cmd.index+1, next_cmd)) { - return true; + // set velocity to zero and stop rotating if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - update_time_ms > get_timeout_ms()) { + guided_vel_target_cms.zero(); + guided_accel_target_cmss.zero(); + if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { + auto_yaw.set_rate(0.0f); + } } - // whether vehicle should stop at the target position depends upon the next command - switch (next_cmd.id) { - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TIME: { - const Location dest_loc = loc_from_cmd(current_cmd, default_loc); - const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); - return wp_nav->set_wp_destination_next_loc(next_dest_loc); - } - case MAV_CMD_NAV_SPLINE_WAYPOINT: { - // get spline's location and next location from command and send to wp_nav - Location next_dest_loc, next_next_dest_loc; - bool next_next_dest_loc_is_spline; - get_spline_from_cmd(next_cmd, default_loc, next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline); - return wp_nav->set_spline_destination_next_loc(next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline); - } - case MAV_CMD_NAV_LAND: - // stop because we may change between rel,abs and terrain alt types - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - case MAV_CMD_NAV_TAKEOFF: - // always stop for RTL and takeoff commands - default: - // for unsupported commands it is safer to stop - break; + // send position and velocity targets to position controller + if (!stabilizing_vel_xy()) { + // set the current commanded xy pos to the target pos and xy vel to the desired vel + guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; + guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; + guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; + guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; + } else if (!stabilizing_pos_xy()) { + // set the current commanded xy pos to the target pos + guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; + guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; + } + pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); + if (!stabilizing_vel_xy()) { + // set position and velocity errors to zero + pos_control->stop_vel_xy_stabilisation(); + } else if (!stabilizing_pos_xy()) { + // set position errors to zero + pos_control->stop_pos_xy_stabilisation(); + } + + // guided_pos_target z-axis should never be a terrain altitude + if (guided_pos_terrain_alt) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } - return true; -} - -// do_land - initiate landing procedure -void ModeAuto::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 - state = State::FlyToLocation; + float pz = guided_pos_target_cm.z; + pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + guided_pos_target_cm.z = pz; - const Location target_loc = terrain_adjusted_location(cmd); + // run position controllers + pos_control->update_xy_controller(); + pos_control->update_z_controller(); - wp_start(target_loc); + // call attitude controller + if (auto_yaw.mode() == AUTO_YAW_HOLD) { + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { - // set landing state - state = State::Descending; - - // initialise landing controller - land_start(); - } -} - -// do_loiter_unlimited - start loitering with no end conditions -// note: caller should set yaw_mode -void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) -{ - // convert back to location - Location target_loc(cmd.content.location); - - // use current location if not provided - if (target_loc.lat == 0 && target_loc.lng == 0) { - // To-Do: make this simpler - Vector3f temp_pos; - copter.wp_nav->get_wp_stopping_point_xy(temp_pos.xy()); - const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN); - target_loc.lat = temp_loc.lat; - target_loc.lng = temp_loc.lng; - } - - // use current altitude if not provided - // To-Do: use z-axis stopping point instead of current alt - if (target_loc.alt == 0) { - // set to current altitude but in command's alt frame - int32_t curr_alt; - if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { - target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); - } else { - // default to current altitude as alt-above-home - target_loc.set_alt_cm(copter.current_loc.alt, - copter.current_loc.get_alt_frame()); + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); + } +} + +// angle_control_run - runs the guided angle controller +// called from guided_run +void ModeGuided::angle_control_run() +{ + // constrain desired lean angles + float roll_in = guided_angle_state.roll_cd; + float pitch_in = guided_angle_state.pitch_cd; + float total_in = norm(roll_in, pitch_in); + float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max); + if (total_in > angle_max) { + float ratio = angle_max / total_in; + roll_in *= ratio; + pitch_in *= ratio; + } + + // wrap yaw request + float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); + float yaw_rate_in = guided_angle_state.yaw_rate_cds; + + float climb_rate_cms = 0.0f; + if (!guided_angle_state.use_thrust) { + // constrain climb rate + climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); + + // get avoidance adjusted climb rate + climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); + } + + // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) { + roll_in = 0.0f; + pitch_in = 0.0f; + climb_rate_cms = 0.0f; + yaw_rate_in = 0.0f; + if (guided_angle_state.use_thrust) { + // initialise vertical velocity controller + pos_control->init_z_controller(); + guided_angle_state.use_thrust = false; } } - // start way point navigator and provide it the desired location - wp_start(target_loc); -} - -// do_circle - initiate moving in a circle -void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) -{ - const Location circle_center = loc_from_cmd(cmd, copter.current_loc); - - // calculate radius - uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 - - // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - circle_movetoedge_start(circle_center, circle_radius_m); -} - -// do_loiter_time - initiate loitering at a point for a given time period -// note: caller should set yaw_mode -void ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd) -{ - // re-use loiter unlimited - do_loiter_unlimited(cmd); - - // setup loiter timer - loiter_time = 0; - loiter_time_max = cmd.p1; // units are (seconds) -} - -// do_loiter_alt - initiate loitering at a point until a given altitude is reached -// note: caller should set yaw_mode -void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) -{ - // re-use loiter unlimited - do_loiter_unlimited(cmd); - _mode = SubMode::LOITER_TO_ALT; - - // if we aren't navigating to a location then we have to adjust - // altitude for current location - Location target_loc(cmd.content.location); - if (target_loc.lat == 0 && target_loc.lng == 0) { - target_loc.lat = copter.current_loc.lat; - target_loc.lng = copter.current_loc.lng; - } - - if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, loiter_to_alt.alt)) { - loiter_to_alt.reached_destination_xy = true; - loiter_to_alt.reached_alt = true; - gcs().send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt"); - return; - } - loiter_to_alt.reached_destination_xy = false; - loiter_to_alt.loiter_start_done = false; - loiter_to_alt.reached_alt = false; - loiter_to_alt.alt_error_cm = 0; - - // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); -} - -// do_spline_wp - initiate move to next waypoint -void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) -{ - // calculate default location used when lat, lon or alt is zero - Location default_loc = copter.current_loc; - if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { - if (!wp_nav->get_wp_destination_loc(default_loc)) { - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } + // interpret positive climb rate or thrust as triggering take-off + const bool positive_thrust_or_climbrate = is_positive(guided_angle_state.use_thrust ? guided_angle_state.thrust : climb_rate_cms); + if (motors->armed() && positive_thrust_or_climbrate) { + copter.set_auto_armed(true); } - // get spline's location and next location from command and send to wp_nav - Location dest_loc, next_dest_loc; - bool next_dest_loc_is_spline; - get_spline_from_cmd(cmd, default_loc, dest_loc, next_dest_loc, next_dest_loc_is_spline); - if (!wp_nav->set_spline_destination_loc(dest_loc, next_dest_loc, next_dest_loc_is_spline)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && !positive_thrust_or_climbrate)) { + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); return; } - _mode = SubMode::WP; - - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; - // this is the delay, stored in seconds - loiter_time_max = cmd.p1; - - // set next destination if necessary - if (!set_next_wp(cmd, dest_loc)) { - // failure to set next destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); + // TODO: use get_alt_hold_state + // landed with positive desired climb rate, takeoff + if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) { + zero_throttle_and_relax_ac(); + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { + set_land_complete(false); + set_throttle_takeoff(); + } return; } - // 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) { - auto_yaw.set_mode_to_default(false); - } -} - -// calculate locations required to build a spline curve from a mission command -// dest_loc is populated from cmd's location using default_loc in cases where the lat and lon or altitude is zero -// next_dest_loc and nest_dest_loc_is_spline is filled in with the following navigation command's location if it exists. If it does not exist it is set to the dest_loc and false -void ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline) -{ - dest_loc = loc_from_cmd(cmd, default_loc); + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // if there is no delay at the end of this segment get next nav command - AP_Mission::Mission_Command temp_cmd; - if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) { - next_dest_loc = loc_from_cmd(temp_cmd, dest_loc); - next_dest_loc_is_spline = temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT; + // call attitude controller + if (guided_angle_state.use_yaw_rate) { + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in); } else { - next_dest_loc = dest_loc; - next_dest_loc_is_spline = false; - } -} - -#if NAV_GUIDED == ENABLED -// do_nav_guided_enable - initiate accepting commands from external nav computer -void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.p1 > 0) { - // start guided within auto - nav_guided_start(); + attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); } -} - -// do_guided_limits - pass guided limits to guided controller -void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) -{ - copter.mode_guided.limit_set( - cmd.p1 * 1000, // convert seconds to ms - cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm - cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm - cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm -} -#endif // NAV_GUIDED - -// do_nav_delay - Delay the next navigation command -void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) -{ - nav_delay_time_start_ms = millis(); - if (cmd.content.nav_delay.seconds > 0) { - // relative delay - nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds + // call position controller + if (guided_angle_state.use_thrust) { + attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt); } else { - // absolute delay to utc time - nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0); + pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms); + pos_control->update_z_controller(); } - gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); -} - -/********************************************************************************/ -// Condition (May) commands -/********************************************************************************/ - -void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd) -{ - condition_start = millis(); - condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) +// helper function to set yaw state and targets +void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { - condition_value = cmd.content.distance.meters * 100; -} - -void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) -{ - auto_yaw.set_fixed_yaw( - cmd.content.yaw.angle_deg, - cmd.content.yaw.turn_rate_dps, - cmd.content.yaw.direction, - cmd.content.yaw.relative_angle > 0); -} - -/********************************************************************************/ -// Do (Now) commands -/********************************************************************************/ - - - -void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.content.speed.target_ms > 0) { - if (cmd.content.speed.speed_type == 2) { - copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f); - } else if (cmd.content.speed.speed_type == 3) { - copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f); - } else { - copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f); - } + if (use_yaw && relative_angle) { + auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle); + } else if (use_yaw && use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, yaw_rate_cds * 0.01f); + } else if (use_yaw && !use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, 0.0f); + } else if (use_yaw_rate) { + auto_yaw.set_rate(yaw_rate_cds); } } -void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { - if (!copter.set_home_to_current_location(false)) { - // ignore failure - } - } else { - if (!copter.set_home(cmd.content.location, false)) { - // ignore failure - } - } -} - -// do_roi - starts actions required by MAV_CMD_DO_SET_ROI -// this involves either moving the camera to point at the ROI (region of interest) -// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature -// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint -void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd) -{ - auto_yaw.set_roi(cmd.content.location); -} - -// point the camera to a specified angle -void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) +// returns true if pilot's yaw input should be used to adjust vehicle's heading +bool ModeGuided::use_pilot_yaw(void) const { -#if HAL_MOUNT_ENABLED - // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead - if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && - !copter.camera_mount.has_pan_control()) { - auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f); - } - // pass the target angles to the camera mount - copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); -#endif + return (copter.g2.guided_options.get() & uint32_t(Options::IgnorePilotYaw)) == 0; } -#if WINCH_ENABLED == ENABLED -// control winch based on mission command -void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) -{ - // Note: we ignore the gripper num parameter because we only support one gripper - switch (cmd.content.winch.action) { - case WINCH_RELAXED: - g2.winch.relax(); - break; - case WINCH_RELATIVE_LENGTH_CONTROL: - g2.winch.release_length(cmd.content.winch.release_length); - break; - case WINCH_RATE_CONTROL: - g2.winch.set_desired_rate(cmd.content.winch.release_rate); - break; - default: - // do nothing - break; - } -} -#endif +// Guided Limit code -// do_payload_place - initiate placing procedure -void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) +// limit_clear - clear/turn off guided limits +void ModeGuided::limit_clear() { - // 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 - nav_payload_place.state = PayloadPlaceStateType_FlyToLocation; - - const Location target_loc = terrain_adjusted_location(cmd); - - wp_start(target_loc); - } else { - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - - // initialise placing controller - payload_place_start(); - } - nav_payload_place.descend_max = cmd.p1; + guided_limit.timeout_ms = 0; + guided_limit.alt_min_cm = 0.0f; + guided_limit.alt_max_cm = 0.0f; + guided_limit.horiz_max_cm = 0.0f; } -// do_RTL - start Return-to-Launch -void ModeAuto::do_RTL(void) +// limit_set - set guided timeout and movement limits +void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { - // start rtl in auto flight mode - rtl_start(); + guided_limit.timeout_ms = timeout_ms; + guided_limit.alt_min_cm = alt_min_cm; + guided_limit.alt_max_cm = alt_max_cm; + guided_limit.horiz_max_cm = horiz_max_cm; } -/********************************************************************************/ -// Verify Nav (Must) commands -/********************************************************************************/ - -// verify_takeoff - check if we have completed the takeoff -bool ModeAuto::verify_takeoff() +// limit_init_time_and_pos - initialise guided start time and position as reference for limit checking +// only called from AUTO mode's auto_nav_guided_start function +void ModeGuided::limit_init_time_and_pos() { - // have we reached our target altitude? - const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); - -#if LANDING_GEAR_ENABLED == ENABLED - // if we have reached our destination - if (reached_wp_dest) { - // retract the landing gear - copter.landinggear.retract_after_takeoff(); - } -#endif + // initialise start time + guided_limit.start_time = AP_HAL::millis(); - return reached_wp_dest; + // initialise start position from current position + guided_limit.start_pos = inertial_nav.get_position(); } -// verify_land - returns true if landing has been completed -bool ModeAuto::verify_land() +// limit_check - returns true if guided mode has breached a limit +// used when guided is invoked from the NAV_GUIDED_ENABLE mission command +bool ModeGuided::limit_check() { - bool retval = false; - - switch (state) { - case State::FlyToLocation: - // check if we've reached the location - if (copter.wp_nav->reached_wp_destination()) { - // get destination so we can use it for loiter target - const Vector2f& dest = copter.wp_nav->get_wp_destination().xy(); - - // initialise landing controller - land_start(dest); - - // advance to next state - state = State::Descending; - } - break; - - case State::Descending: - // rely on THROTTLE_LAND mode to correctly update landing status - retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); - if (retval && !mission.continue_after_land() && copter.motors->armed()) { - /* - we want to stop mission processing on land - completion. Disarm now, then return false. This - leaves mission state machine in the current NAV_LAND - mission item. After disarming the mission will reset - */ - copter.arming.disarm(AP_Arming::Method::LANDED); - retval = false; - } - break; - - default: - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - retval = true; - break; - } - - // true is returned if we've successfully landed - return retval; -} - -#define NAV_PAYLOAD_PLACE_DEBUGGING 0 - -#if NAV_PAYLOAD_PLACE_DEBUGGING -#include -#define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) -#else -#define debug(fmt, args ...) -#endif - -// verify_payload_place - returns true if placing has been completed -bool ModeAuto::verify_payload_place() -{ - const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds - const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds - const float hover_throttle_placed_fraction = 0.7; // i.e. if throttle is less than 70% of hover we have placed - const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed - const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed - - const float current_throttle_level = motors->get_throttle(); - const uint32_t now = AP_HAL::millis(); - - // if we discover we've landed then immediately release the load: - if (copter.ap.land_complete) { - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - case PayloadPlaceStateType_Calibrating_Hover_Start: - case PayloadPlaceStateType_Calibrating_Hover: - case PayloadPlaceStateType_Descending_Start: - case PayloadPlaceStateType_Descending: - gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: landed"); - nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; - break; - case PayloadPlaceStateType_Releasing_Start: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Released: - case PayloadPlaceStateType_Ascending_Start: - case PayloadPlaceStateType_Ascending: - case PayloadPlaceStateType_Done: - break; - } - } - - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - if (!copter.wp_nav->reached_wp_destination()) { - return false; - } - payload_place_start(); - return false; - case PayloadPlaceStateType_Calibrating_Hover_Start: - // hover for 1 second to get an idea of what our hover - // throttle looks like - debug("Calibrate start"); - nav_payload_place.hover_start_timestamp = now; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover; - FALLTHROUGH; - case PayloadPlaceStateType_Calibrating_Hover: { - if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) { - // still calibrating... - debug("Calibrate Timer: %d", now - nav_payload_place.hover_start_timestamp); - return false; - } - // we have a valid calibration. Hopefully. - nav_payload_place.hover_throttle_level = current_throttle_level; - const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors->get_throttle_hover()); - gcs().send_text(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast(hover_throttle_delta)); - nav_payload_place.state = PayloadPlaceStateType_Descending_Start; - } - FALLTHROUGH; - case PayloadPlaceStateType_Descending_Start: - nav_payload_place.descend_start_timestamp = now; - nav_payload_place.descend_start_altitude = inertial_nav.get_altitude(); - nav_payload_place.descend_throttle_level = 0; - nav_payload_place.state = PayloadPlaceStateType_Descending; - FALLTHROUGH; - case PayloadPlaceStateType_Descending: - // make sure we don't descend too far: - debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max); - if (!is_zero(nav_payload_place.descend_max) && - nav_payload_place.descend_start_altitude - inertial_nav.get_altitude() > nav_payload_place.descend_max) { - nav_payload_place.state = PayloadPlaceStateType_Ascending; - gcs().send_text(MAV_SEVERITY_WARNING, "Reached maximum descent"); - return false; // we'll do any cleanups required next time through the loop - } - // see if we've been descending long enough to calibrate a descend-throttle-level: - if (is_zero(nav_payload_place.descend_throttle_level) && - now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time) { - nav_payload_place.descend_throttle_level = current_throttle_level; - } - // watch the throttle to determine whether the load has been placed - // debug("hover ratio: %f descend ratio: %f\n", current_throttle_level/nav_payload_place.hover_throttle_level, ((nav_payload_place.descend_throttle_level == 0) ? -1.0f : current_throttle_level/nav_payload_place.descend_throttle_level)); - if (current_throttle_level/nav_payload_place.hover_throttle_level > hover_throttle_placed_fraction && - (is_zero(nav_payload_place.descend_throttle_level) || - current_throttle_level/nav_payload_place.descend_throttle_level > descent_throttle_placed_fraction)) { - // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid) - nav_payload_place.place_start_timestamp = 0; - return false; - } - if (nav_payload_place.place_start_timestamp == 0) { - // we've only just now hit the correct throttle level - nav_payload_place.place_start_timestamp = now; - return false; - } else if (now - nav_payload_place.place_start_timestamp < placed_time) { - // keep going down.... - debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp); - return false; - } - nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; - FALLTHROUGH; - case PayloadPlaceStateType_Releasing_Start: -#if GRIPPER_ENABLED == ENABLED - if (g2.gripper.valid()) { - gcs().send_text(MAV_SEVERITY_INFO, "Releasing the gripper"); - g2.gripper.release(); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "Gripper not valid"); - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - break; - } -#else - gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled"); -#endif - nav_payload_place.state = PayloadPlaceStateType_Releasing; - FALLTHROUGH; - case PayloadPlaceStateType_Releasing: -#if GRIPPER_ENABLED == ENABLED - if (g2.gripper.valid() && !g2.gripper.released()) { - return false; - } -#endif - nav_payload_place.state = PayloadPlaceStateType_Released; - FALLTHROUGH; - case PayloadPlaceStateType_Released: { - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - } - FALLTHROUGH; - case PayloadPlaceStateType_Ascending_Start: { - Location target_loc(inertial_nav.get_position(), Location::AltFrame::ABOVE_ORIGIN); - target_loc.alt = nav_payload_place.descend_start_altitude; - wp_start(target_loc); - nav_payload_place.state = PayloadPlaceStateType_Ascending; - } - FALLTHROUGH; - case PayloadPlaceStateType_Ascending: - if (!copter.wp_nav->reached_wp_destination()) { - return false; - } - nav_payload_place.state = PayloadPlaceStateType_Done; - FALLTHROUGH; - case PayloadPlaceStateType_Done: + // check if we have passed the timeout + if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { return true; - default: - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return true; - } - // should never get here - return true; -} -#undef debug - -bool ModeAuto::verify_loiter_unlimited() -{ - return false; -} - -// verify_loiter_time - check if we have loitered long enough -bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) -{ - // return immediately if we haven't reached our destination - if (!copter.wp_nav->reached_wp_destination()) { - return false; } - // start our loiter timer - if ( loiter_time == 0 ) { - loiter_time = millis(); - } + // get current location + const Vector3f& curr_pos = inertial_nav.get_position(); - // check if loiter timer has run out - if (((millis() - loiter_time) / 1000) >= loiter_time_max) { - gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); + // 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; } - return false; -} - -// verify_loiter_to_alt - check if we have reached both destination -// (roughly) and altitude (precisely) -bool ModeAuto::verify_loiter_to_alt() const -{ - if (loiter_to_alt.reached_destination_xy && - loiter_to_alt.reached_alt) { + // 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; } - return false; -} -// verify_RTL - handles any state changes required to implement RTL -// do_RTL should have been called once first to initialise all variables -// returns true with RTL has completed successfully -bool ModeAuto::verify_RTL() -{ - return (copter.mode_rtl.state_complete() && - (copter.mode_rtl.state() == ModeRTL::SubMode::FINAL_DESCENT || copter.mode_rtl.state() == ModeRTL::SubMode::LAND) && - (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE)); -} - -/********************************************************************************/ -// Verify Condition (May) commands -/********************************************************************************/ - -bool ModeAuto::verify_wait_delay() -{ - if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) { - condition_value = 0; - return true; + // check if we have gone beyond horizontal limit + if (guided_limit.horiz_max_cm > 0.0f) { + float horiz_move = 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; } -bool ModeAuto::verify_within_distance() +const Vector3p &ModeGuided::get_target_pos() const { - if (wp_distance() < (uint32_t)MAX(condition_value,0)) { - condition_value = 0; - return true; - } - return false; + return guided_pos_target_cm; } -// verify_yaw - return true if we have reached the desired heading -bool ModeAuto::verify_yaw() +const Vector3f& ModeGuided::get_target_vel() const { - // 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_FIXED) { - auto_yaw.set_mode(AUTO_YAW_FIXED); - } - - // check if we are within 2 degrees of the target heading - return (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200); + return guided_vel_target_cms; } -// verify_nav_wp - check if we have reached the next way point -bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) +const Vector3f& ModeGuided::get_target_accel() const { - // check if we have reached the waypoint - if ( !copter.wp_nav->reached_wp_destination() ) { - return false; - } - - // start timer if necessary - if (loiter_time == 0) { - loiter_time = millis(); - if (loiter_time_max > 0) { - // play a tone - AP_Notify::events.waypoint_complete = 1; - } - } - - // check if timer has run out - if (((millis() - loiter_time) / 1000) >= loiter_time_max) { - if (loiter_time_max == 0) { - // play a tone - AP_Notify::events.waypoint_complete = 1; - } - gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); - return true; - } - return false; + return guided_accel_target_cmss; } -// verify_circle - check if we have circled the point enough -bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) +uint32_t ModeGuided::wp_distance() const { - // check if we've reached the edge - if (mode() == SubMode::CIRCLE_MOVE_TO_EDGE) { - if (copter.wp_nav->reached_wp_destination()) { - // start circling - circle_start(); - } - return false; + switch(guided_mode) { + case SubMode::WP: + return wp_nav->get_wp_distance_to_destination(); + case SubMode::Pos: + return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y); + case SubMode::PosVelAccel: + return pos_control->get_pos_error_xy_cm(); + break; + default: + return 0; } - - // check if we have completed circling - return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); } -// verify_spline_wp - check if we have reached the next way point using spline -bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) +int32_t ModeGuided::wp_bearing() const { - // check if we have reached the waypoint - if ( !copter.wp_nav->reached_wp_destination() ) { - return false; - } - - // start timer if necessary - if (loiter_time == 0) { - loiter_time = millis(); - } - - // check if timer has run out - if (((millis() - loiter_time) / 1000) >= loiter_time_max) { - gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); - return true; + switch(guided_mode) { + case SubMode::WP: + return wp_nav->get_wp_bearing_to_destination(); + case SubMode::Pos: + return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat()); + case SubMode::PosVelAccel: + return pos_control->get_bearing_to_target_cd(); + break; + case SubMode::TakeOff: + case SubMode::Accel: + case SubMode::VelAccel: + case SubMode::Angle: + // these do not have bearings + return 0; } - return false; + // compiler guarantees we don't get here + return 0.0; } -#if NAV_GUIDED == ENABLED -// verify_nav_guided - check if we have breached any limits -bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +float ModeGuided::crosstrack_error() const { - // if disabling guided mode then immediately return true so we move to next command - if (cmd.p1 == 0) { - return true; + switch (guided_mode) { + case SubMode::WP: + return wp_nav->crosstrack_error(); + case SubMode::Pos: + case SubMode::TakeOff: + case SubMode::Accel: + case SubMode::VelAccel: + case SubMode::PosVelAccel: + return pos_control->crosstrack_error(); + case SubMode::Angle: + // no track to have a crosstrack to + return 0; } - - // check time and position limits - return copter.mode_guided.limit_check(); + // compiler guarantees we don't get here + return 0; } -#endif // NAV_GUIDED -// verify_nav_delay - check if we have waited long enough -bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) +// return guided mode timeout in milliseconds. Only used for velocity, acceleration and angle control +uint32_t ModeGuided::get_timeout_ms() const { - if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) { - nav_delay_time_max_ms = 0; - return true; - } - return false; + return MAX(copter.g2.guided_timeout, 0.1) * 1000; } #endif diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a1950e3141..4359dfc5d9 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -169,7 +169,7 @@ void ModeGuided::wp_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { @@ -648,7 +648,7 @@ void ModeGuided::pos_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { @@ -707,7 +707,7 @@ void ModeGuided::accel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { @@ -771,7 +771,7 @@ void ModeGuided::velaccel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { @@ -847,7 +847,7 @@ void ModeGuided::posvelaccel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!copter.failsafe.radio && use_pilot_yaw()) { + if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) { //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制 // 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)) { diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index bd1233351c..edbaae52c9 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -186,6 +186,34 @@ void Copter::zr_app_50hz(){ } } break; + case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE: + { + + if(control_mode != Mode::Number::GUIDED){ + set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING); + } + uint8_t user_mode = (data.x); + if(user_mode == 3 || user_mode == 4 || user_mode == 6 || user_mode == 17){ + bool change_ok = set_mode(user_mode, ModeReason::SCRIPTING); + + if(zr_serial_api.get_param_debug() && change_ok){ + // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt); + snprintf(buf, sizeof(buf), "Change mode %d",user_mode); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } + }else{ + if(zr_serial_api.get_param_debug()){ + // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt); + snprintf(buf, sizeof(buf), "Unsupported mode %d",user_mode); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } + } + + } + break; + default: break; diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index eaa6531f2d..6cbbe8d6b5 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #define ZR_API_DEBUG 1 @@ -294,6 +295,15 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l if(control_data_last_time == last_time){ // 数据没更新，直接退出 return false; } + AP::logger().Write("APID", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBiiif", + AP_HAL::micros64(), //Q + mode, //B + flight_control_parser.msg.type, //B + flight_control_parser.msg.x, //i + flight_control_parser.msg.y, //i + flight_control_parser.msg.z, //i + flight_control_parser.msg.yaw); //f + last_time = control_data_last_time; WITH_SEMAPHORE(sem); if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头 @@ -316,7 +326,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l return false; // 校验失败 } type = flight_control_parser.msg.type; - if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){ + if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_MODE ){ set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误 if(get_param_debug()){ @@ -324,7 +334,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l } return false; // 控制类型错误 } - if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){ + if(type != ZR_Msg_Type::MSG_CONTROL_MODE && type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4 ){ set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); if(get_param_debug()){ Debug("mode error: %d",mode); diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index 351b2b201b..921e705d43 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -46,7 +46,8 @@ public: MSG_CONTROL_TKOFF , // 飞行控制：起飞 MSG_CONTROL_POS , // 飞行控制：位置控制模式 MSG_CONTROL_VEL , // 飞行控制：速度控制模式 - MSG_CONTROL_VEL_P // 飞行控制：速度控制模式 + MSG_CONTROL_VEL_P, // 飞行控制：速度控制模式 + MSG_CONTROL_MODE, // 飞行控制：速度控制模式 }; @@ -82,7 +83,7 @@ public: uint8_t get_param_debug(){ return parm_msg_debug; }; void print_data(const char *fmt, ...) const; void print_msg(const char *msg) const; - + static const struct AP_Param::GroupInfo var_info[]; protected: AP_Int8 parm_msg_debug;