diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1d1c86c64714b..056c3d9b0e7fb 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -52,6 +52,7 @@ #include // needed for AHRS build #include // inertial navigation library #include // ArduCopter waypoint navigation library +#include #include // ArduCopter Loiter Mode Library #include // circle navigation library #include // ArduPilot Mega Declination Helper Library @@ -69,6 +70,8 @@ #include // Crop sprayer library #include // ADS-B RF based collision avoidance module library #include // ArduPilot proximity sensor library +#include +#include // Configuration #include "defines.h" @@ -80,6 +83,12 @@ #define AC_AttitudeControl_t AC_AttitudeControl_Multi #endif +#if FRAME_CONFIG == HELI_FRAME + #define AC_WPNav_t AC_WPNav_Heli +#else + #define AC_WPNav_t AC_WPNav +#endif + #if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else @@ -461,13 +470,18 @@ class Copter : public AP_Vehicle { // To-Do: move inertial nav up or other navigation variables down here AC_AttitudeControl_t *attitude_control; AC_PosControl *pos_control; - AC_WPNav *wp_nav; + AC_WPNav_t *wp_nav; AC_Loiter *loiter_nav; #if MODE_CIRCLE_ENABLED == ENABLED AC_Circle *circle_nav; #endif + AP_SpdHgtControl_Heli helispdhgtctrl{ahrs}; + + // L1 Controller and SpdHgt controller declarations + AP_L1_Control_Heli L1_controller{ahrs, &helispdhgtctrl}; + // System Timers // -------------- // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4d6883d18b492..5c2d922472e5d 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -493,8 +493,12 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(ins, "INS_", AP_InertialSensor), // @Group: WPNAV_ - // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp + // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp,../libraries/AC_WPNav/AC_WPNav_Heli.cpp +#if FRAME_CONFIG == HELI_FRAME + GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav_Heli), +#else GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav), +#endif // @Group: LOIT_ // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp @@ -1137,6 +1141,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel), + // @Group: NAVL1_ + // @Path: ../libraries/AP_L1_Control/AP_L1_Control_Heli.cpp + AP_SUBGROUPPTR(L1_controller_ptr, "NAVL1_", 57, ParametersG2, AP_L1_Control_Heli), + + // @Group: SPDHGT_ + // @Path: ../libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.cpp + AP_SUBGROUPPTR(helispdhgtctrl_ptr, "SPDHGT_", 58, ParametersG2, AP_SpdHgtControl_Heli), + AP_GROUPEND }; @@ -1191,6 +1203,9 @@ ParametersG2::ParametersG2(void) #endif ,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) + + ,L1_controller_ptr(&copter.L1_controller) + ,helispdhgtctrl_ptr(&copter.helispdhgtctrl) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index cf6c92a9a4c6f..3bc14ef82acc3 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -663,6 +663,10 @@ class ParametersG2 { AP_Float guided_timeout; #endif + void *L1_controller_ptr; + void *helispdhgtctrl_ptr; + + AP_Int8 surftrak_mode; AP_Int8 failsafe_dr_enable; AP_Int16 failsafe_dr_timeout; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 7d5f6e937111a..bad2298767fbf 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -166,7 +166,7 @@ class Mode { // convenience references to avoid code churn in conversion: Parameters &g; ParametersG2 &g2; - AC_WPNav *&wp_nav; + AC_WPNav_t *&wp_nav; AC_Loiter *&loiter_nav; AC_PosControl *&pos_control; AP_InertialNav &inertial_nav; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 7f059fbf6b8c5..3d11b0d76bab5 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -975,13 +975,37 @@ void ModeAuto::wp_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) { + copter.failsafe_terrain_set_status(wp_nav->update_l1_wpnav()); + } else { + // run waypoint controller + copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + } +#else // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); +#endif // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle pos_control->update_z_controller(); +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) { + target_yaw_rate += wp_nav->get_yaw_rate(); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_l1_roll(), wp_nav->get_l1_pitch(), target_yaw_rate); + } else { + // 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 { + // 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()); + } + } +#else // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot @@ -990,6 +1014,7 @@ void ModeAuto::wp_run() // 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()); } +#endif } // auto_land_run - lands in auto mode @@ -1368,14 +1393,17 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) // 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) { + wp_nav->set_stopping_at_wp(false); // do not add next wp if current command has a delay meaning the vehicle will stop at the destination if (current_cmd.p1 > 0) { + wp_nav->set_stopping_at_wp(true); return true; } // 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)) { + wp_nav->set_stopping_at_wp(true); return true; } @@ -1489,8 +1517,18 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) circle_radius_m *= 10; } +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation()) { + // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge + circle_movetoedge_start(circle_center, 0); + } else { + // 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); + } +#else // 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); +#endif } // do_loiter_time - initiate loitering at a point for a given time period @@ -1544,7 +1582,8 @@ 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->is_active() && wp_nav->reached_wp_destination()) + || (wp_nav->is_L1_active() && wp_nav->reached_l1_destination())) { if (!wp_nav->get_wp_destination_loc(default_loc)) { // this should never happen INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -1831,6 +1870,26 @@ bool ModeAuto::verify_land() switch (state) { case State::FlyToLocation: +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation()) { + if (copter.wp_nav->reached_l1_destination()) { + // initialise landing controller + land_start(); + + // advance to next state + state = State::Descending; + } + } else { + // check if we've reached the location + if (copter.wp_nav->reached_wp_destination()) { + // initialise landing controller + land_start(); + + // advance to next state + state = State::Descending; + } + } +#else // check if we've reached the location if (copter.wp_nav->reached_wp_destination()) { // initialise landing controller @@ -1839,6 +1898,7 @@ bool ModeAuto::verify_land() // advance to next state state = State::Descending; } +#endif break; case State::Descending: @@ -2039,10 +2099,24 @@ bool ModeAuto::verify_loiter_unlimited() // verify_loiter_time - check if we have loitered long enough bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) { +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation()) { + // check if we have reached the waypoint + if ( !copter.wp_nav->reached_l1_destination() ) { + return false; + } + } else { + // return immediately if we haven't reached our destination + if (!copter.wp_nav->reached_wp_destination()) { + return false; + } + } +#else // return immediately if we haven't reached our destination if (!copter.wp_nav->reached_wp_destination()) { return false; } +#endif // start our loiter timer if ( loiter_time == 0 ) { @@ -2114,7 +2188,17 @@ bool ModeAuto::verify_yaw() // verify_nav_wp - check if we have reached the next way point bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { - // check if we have reached the waypoint +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation() && !wp_nav->is_spline()) { + // check if we have reached the waypoint + if ( !copter.wp_nav->reached_l1_destination() ) { + return false; + } + gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index); + return true; + } else { +#endif + // check if we have reached the waypoint if ( !copter.wp_nav->reached_wp_destination() ) { return false; } @@ -2138,11 +2222,37 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) return true; } return false; +#if FRAME_CONFIG == HELI_FRAME + } +#endif + } // verify_circle - check if we have circled the point enough bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) { +#if FRAME_CONFIG == HELI_FRAME + if (wp_nav->use_l1_navigation()) { + // check if we have reached the waypoint + if ( !copter.wp_nav->reached_l1_destination()) { + return false; + } + // check if we have completed circling + return fabsf((float)copter.wp_nav->get_angle_total()/36000.0f) >= LOWBYTE(cmd.p1); + } else { + // 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; + } + + // check if we have completed circling + return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1); + } +#else // check if we've reached the edge if (_mode == SubMode::CIRCLE_MOVE_TO_EDGE) { if (copter.wp_nav->reached_wp_destination()) { @@ -2154,6 +2264,7 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) // check if we have completed circling return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1); +#endif } // verify_spline_wp - check if we have reached the next way point using spline diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 0c0143a3a906f..8c50d841261b8 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -68,6 +68,7 @@ void Copter::init_ardupilot() #endif #if FRAME_CONFIG == HELI_FRAME input_manager.set_loop_rate(scheduler.get_loop_rate_hz()); + helispdhgtctrl.set_dt(scheduler.get_loop_period_s()); #endif init_rc_in(); // sets up rc channels from radio @@ -466,10 +467,14 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); +#if FRAME_CONFIG != HELI_FRAME #if AC_OAPATHPLANNER_ENABLED == ENABLED wp_nav = new AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control); #else wp_nav = new AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); +#endif +#else + wp_nav = new AC_WPNav_Heli(inertial_nav, *ahrs_view, *pos_control, *attitude_control, &helispdhgtctrl, L1_controller); #endif if (wp_nav == nullptr) { AP_BoardConfig::allocation_error("WPNav"); diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 863834ad4ed7d..e015549eb9997 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -12,6 +12,8 @@ def build(bld): 'AC_PrecLand', 'AC_Sprayer', 'AC_Autorotation', + 'AP_SpdHgtControl', + 'AP_L1_Control', 'AC_WPNav', 'AP_Camera', 'AP_IRLock', diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 13fdc4e17193c..77dcea1967314 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -52,7 +52,7 @@ class AC_WPNav /// speed_cms is the desired max speed to travel between waypoints. should be a positive value or omitted to use the default speed /// updates target roll, pitch targets and I terms based on vehicle lean angles /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination - void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}); + virtual void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}); /// set current target horizontal speed during wp navigation void set_speed_xy(float speed_cms); @@ -97,8 +97,8 @@ class AC_WPNav /// set_wp_destination waypoint using location class /// provide the next_destination if known /// returns false if conversion from location to vector from ekf origin cannot be calculated - bool set_wp_destination_loc(const Location& destination); - bool set_wp_destination_next_loc(const Location& destination); + virtual bool set_wp_destination_loc(const Location& destination); + virtual bool set_wp_destination_next_loc(const Location& destination); // get destination as a location. Altitude frame will be absolute (AMSL) or above terrain // returns false if unable to return a destination (for example if origin has not yet been set) @@ -111,7 +111,7 @@ class AC_WPNav /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain virtual bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false); - bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false); + virtual bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false); /// set waypoint destination using NED position vector from ekf origin in meters /// provide next_destination_NED if known @@ -156,6 +156,9 @@ class AC_WPNav // returns true if update_wpnav has been run very recently bool is_active() const; + // returns true if this leg is spline + bool is_spline() { return _this_leg_is_spline; } + /// /// spline methods /// diff --git a/libraries/AC_WPNav/AC_WPNav_Heli.cpp b/libraries/AC_WPNav/AC_WPNav_Heli.cpp new file mode 100644 index 0000000000000..0fa7880abbe23 --- /dev/null +++ b/libraries/AC_WPNav/AC_WPNav_Heli.cpp @@ -0,0 +1,331 @@ +#include "AC_WPNav_Heli.h" +#include +#include +#include + +// table of user settable parameters +const AP_Param::GroupInfo AC_WPNav_Heli::var_info[] = { + // parameters from parent vehicle + AP_NESTEDGROUPINFO(AC_WPNav, 0), + + // @Param: USE_L1_NAV + // @DisplayName: Waypoint mission use L1 navigation with speed/height control + // @Description: This controls if waypoint missions use L1 navigation + // @Values: 0:Disable,1:Enable + // @User: Standard + AP_GROUPINFO("USE_L1_NAV", 1, AC_WPNav_Heli, _l1_nav_use, 0), + + // @Param: L1_LOIT_RAD + // @DisplayName: Loiter Radius for L1 Navigation + // @Description: This sets the circle radius for the L1 loiter mode + // @Range: 25 200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("L1_LTR_RAD", 2, AC_WPNav_Heli, _loiter_radius, 50.0f), + + AP_GROUPEND +}; + +AC_WPNav_Heli::AC_WPNav_Heli( const AP_InertialNav& inav, + const AP_AHRS_View& ahrs, + AC_PosControl& pos_control, + const AC_AttitudeControl& attitude_control, + AP_SpdHgtControl_Heli *helispdhgtctrl, + AP_L1_Control_Heli& L1_controller) : + AC_WPNav(inav,ahrs,pos_control,attitude_control), + _helispdhgtctrl(helispdhgtctrl), + _L1_controller(L1_controller) +{ + AP_Param::setup_object_defaults(this, var_info); +} + + +/// wp_and_spline_init - initialise straight line and spline waypoint controllers +/// updates target roll, pitch targets and I terms based on vehicle lean angles +/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination +void AC_WPNav_Heli::wp_and_spline_init(float speed_cms, Vector3f stopping_point) +{ + + AC_WPNav::wp_and_spline_init(speed_cms, stopping_point); + + // if waypoint controller is not active, set origin to current position + _prev_WP_pos = _inav.get_position_neu_cm(); + + _helispdhgtctrl->init_controller(); + _helispdhgtctrl->set_desired_speed(_wp_speed_cms); + _helispdhgtctrl->set_max_accel(_wp_accel_cmss); + +} + +/// set_wp_destination waypoint using location class +/// returns false if conversion from location to vector from ekf origin cannot be calculated +bool AC_WPNav_Heli::set_wp_destination_loc(const Location& destination) +{ + + return AC_WPNav::set_wp_destination_loc(destination); + +} + +/// set next destination using location class +/// returns false if conversion from location to vector from ekf origin cannot be calculated +bool AC_WPNav_Heli::set_wp_destination_next_loc(const Location& destination) +{ + + return AC_WPNav::set_wp_destination_next_loc(destination); + +} + +/// set_wp_destination waypoint using position vector (distance from home in cm) +/// terrain_alt should be true if destination.z is a desired altitude above terrain +bool AC_WPNav_Heli::set_wp_destination(const Vector3f& destination, bool terrain_alt) +{ + + // set destination wp location for L1 Navigation + set_L1_wp_origin_and_destination(destination); + + return AC_WPNav::set_wp_destination(destination, terrain_alt); + +} + +/// set next destination using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination.z is a desired altitude above terrain +/// provide next_destination +bool AC_WPNav_Heli::set_wp_destination_next(const Vector3f& destination, bool terrain_alt) +{ + + // set next wp destination + _next_WP_pos = destination; + + return AC_WPNav::set_wp_destination_next(destination, terrain_alt); + +} + + + +/// set the L1 navigation controller origin and destination +void AC_WPNav_Heli::set_L1_wp_origin_and_destination(const Vector3f& destination) +{ + + // if waypoint controller is active use the existing position target as the origin + if ((AP_HAL::millis() - _wp_last_l1_update) < 1000) { + _prev_WP_pos = _this_WP_pos; + } else { + // if waypoint controller is not active, set origin to current position + _prev_WP_pos = _inav.get_position_neu_cm(); + } + _this_WP_pos = destination; + +// int32_t temp_alt; +// destination.get_alt_cm(Location::ALT_FRAME_ABOVE_ORIGIN, temp_alt); +// _pos_control.set_alt_target(temp_alt); + + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: prev wp pos x:%f y:%f", double(_prev_WP_pos.x), double(_prev_WP_pos.y)); + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: this wp pos x:%f y:%f", double(_this_WP_pos.x), double(_this_WP_pos.y)); + + _reached_l1_destination = false; + +} + +/// update_wpnav - run the wp controller - should be called at 100hz or higher +bool AC_WPNav_Heli::update_l1_wpnav() +{ + bool ret = true; + _desired_speed = _wp_speed_cms; + + AP_Mission *_mission = AP_Mission::get_singleton(); + // get dt from pos controller + float dt = _pos_control.get_dt(); + + // advance the target if necessary + if (!advance_l1_wp_target_along_track(dt)) { + // To-Do: handle inability to advance along track (probably because of missing terrain data) + ret = false; + } + + // wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested +// wp_speed_update(dt); + + // run plane waypoint controller + AP_Mission::Mission_Command cmd; + cmd = _mission->get_current_nav_cmd(); + if (cmd.id == MAV_CMD_NAV_LOITER_UNLIM || cmd.id == MAV_CMD_NAV_LOITER_TIME) { + _l1_loiter_type = 1; + } else if (cmd.id == MAV_CMD_NAV_LOITER_TURNS) { + _l1_loiter_type = 2; + } else { + _l1_loiter_type = 0; + } + + _helispdhgtctrl->set_max_accel(_wp_accel_cmss); + Vector3f curr_pos = _inav.get_position_neu_cm(); + float speed_forward = _inav.get_velocity_xy_cms().x*_ahrs.cos_yaw() + _inav.get_velocity_xy_cms().y*_ahrs.sin_yaw(); + Vector2f dist_vec = curr_pos.xy() - _this_WP_pos.xy(); + float dist = dist_vec.length(); + float stop_distance = 0.6f * sq(speed_forward) / _wp_accel_cmss; + + if (_l1_loiter_type > 0) { + _L1_controller.update_loiter(_this_WP_pos.xy(), _loiter_radius, 1); + _stopping_at_waypoint = false; + } + + // if last nav command is waypoint then stop at waypoint + + if (dist < stop_distance || _stopping_at_waypoint) { + _desired_speed = 100.0f; + } + + + _helispdhgtctrl->set_desired_speed(_desired_speed); + _helispdhgtctrl->update_speed_controller(); + _wp_last_l1_update = AP_HAL::millis(); + + return ret; +} + + +bool AC_WPNav_Heli::advance_l1_wp_target_along_track(float dt) +{ + static bool print_text = false; + static uint16_t pcnt = 2000; + // set up for altitude calculation + int32_t temp_alt; + int32_t wp_alt = int32_t(_this_WP_pos.z); + int32_t prev_wp_alt = int32_t(_prev_WP_pos.z); + Vector3f curr_pos = _inav.get_position_neu_cm(); + Vector2f prev_wp_to_curr = curr_pos.xy() - _prev_WP_pos.xy(); + float dist_to_curr_loc = prev_wp_to_curr.length(); + Vector2f prev_wp_to_next_wp = _this_WP_pos.xy() - _prev_WP_pos.xy(); + float dist_btwn_wp = prev_wp_to_next_wp.length(); + + if (_l1_loiter_type == 2) { + if (_L1_controller.reached_loiter_target()) { + _L1_controller.loiter_angle_update(); + } else { + _L1_controller.loiter_angle_reset(); + } + } + + if (_l1_loiter_type > 0) { + if (_L1_controller.reached_loiter_target()) { + temp_alt = wp_alt; + _reached_l1_destination = true; + } else { + float radius = _L1_controller.loiter_radius(_loiter_radius); + temp_alt = (wp_alt - prev_wp_alt) * dist_to_curr_loc / (dist_btwn_wp - radius) + prev_wp_alt; + } + } + + Vector3f flex_this_WP_pos = _this_WP_pos; + + _L1_controller.update_waypoint(_prev_WP_pos.xy(), flex_this_WP_pos.xy()); + float acceptance_distance_m = 5.0f; // default to: if overflown - let it fly up to the point + + float speed_forward = _inav.get_velocity_xy_cms().x*_ahrs.cos_yaw() + _inav.get_velocity_xy_cms().y*_ahrs.sin_yaw(); + float stop_distance = 0.6f * sq(speed_forward) / _wp_accel_cmss; + Vector2f currpos_to_nextwp = _this_WP_pos.xy() - curr_pos.xy(); + + if (!_stopping_at_waypoint) { + float next_ground_course_cd = get_bearing_cd(_this_WP_pos.xy(), _next_WP_pos.xy()); + // get the heading of the current leg + float ground_course_cd = get_bearing_cd(_prev_WP_pos.xy(), _this_WP_pos.xy()); + + // work out the angle we need to turn through + float next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f; + float angle_max = 45.0f; + if (print_text) { + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: ground_course:%f next course:%f", double(ground_course_cd * 0.01f), double(next_ground_course_cd * 0.01f)); + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: turn angle:%f", double(180.0f - fabs(next_turn_angle))); + } + + acceptance_distance_m = _L1_controller.turn_distance(_wp_radius_cm * 0.01f, next_turn_angle); + + if (180.0f - fabs(next_turn_angle) < 90) { + float next_turn_radius = sq(0.01f * _wp_speed_cms) * cos(angle_max / 57.3f)/(9.81f*safe_sqrt(1 - sq(cos(angle_max / 57.3)))); + float required_turn_radius = 0.01f * _wp_radius_cm * tan(0.5f * (180.0f - fabs(next_turn_angle)) / 57.3); + if (print_text) { + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: turn radius:%f required radius:%f", double(next_turn_radius), double(required_turn_radius)); + } + if (required_turn_radius < next_turn_radius) { + float turn_speed = 100.0 * safe_sqrt(required_turn_radius * 9.81f / cos(angle_max / 57.3)) * safe_sqrt(safe_sqrt(1 - sq(cos(angle_max / 57.3)))); + float brake_dist = stop_distance - 0.6f * sq(turn_speed) / _wp_accel_cmss; + if (currpos_to_nextwp.length() < acceptance_distance_m * 100.0f + 2.0f * brake_dist) { + // aircraft must slow down to make turn + _desired_speed = turn_speed; + } + if (print_text) { + gcs().send_text(MAV_SEVERITY_INFO, "L1Nav: brake dist:%f acceptance_dist:%f", double(brake_dist), double(acceptance_distance_m * 100.0f)); + } + } + } + } + if (pcnt > 0) { + pcnt -= 1; + print_text = false; + } else { + pcnt = 2000; + print_text = true; + } + + if (_in_turn == true) { + Vector2f currpos_to_turn_wp = _turn_WP_pos.xy() - curr_pos.xy(); + if (currpos_to_turn_wp.length() > _turn_accept_dist) { + _desired_speed = _wp_speed_cms; + _in_turn = false; + } + } + + if ( currpos_to_nextwp.length() <= acceptance_distance_m * 100.0f) { + _reached_l1_destination = true; + if (!_stopping_at_waypoint) { + _in_turn = true; + _turn_WP_pos.xy() = _this_WP_pos.xy(); + _turn_accept_dist = acceptance_distance_m * 100.0f; + } + } + + + + + // have we flown past the waypoint? +// if (location_passed_point(curr_loc, _prev_WP_loc, flex_next_WP_loc)) { +// _reached_l1_destination = true; +// } + + if (wp_alt == prev_wp_alt) { + temp_alt = wp_alt; + } else { + temp_alt = (wp_alt - prev_wp_alt) * dist_to_curr_loc / dist_btwn_wp + prev_wp_alt; + } +// break; + +// } + + // don't allow temp_alt to beyond wp altitudes + if (wp_alt > prev_wp_alt) { + temp_alt = constrain_float(temp_alt, prev_wp_alt, wp_alt); + } else if (prev_wp_alt > wp_alt) { + temp_alt = constrain_float(temp_alt, wp_alt, prev_wp_alt); + } + _pos_control.set_alt_target_with_slew(temp_alt); + + return true; +} + +// returns true if update_wpnav has been run very recently +bool AC_WPNav_Heli::is_L1_active() const +{ + return (AP_HAL::millis() - _wp_last_l1_update) < 200; +} + +/// using_l1_navigation - true when using L1 navigation controller +bool AC_WPNav_Heli::use_l1_navigation() +{ +// float speed_forward = _inav.get_velocity_xy_cms().x*_ahrs.cos_yaw() + _inav.get_velocity_xy_cms().y*_ahrs.sin_yaw(); +// Vector2f curr_vel = _inav.get_velocity_xy_cms(); +// float speed_forward = curr_vel.length(); + if (_l1_nav_use == 1) { + return true; + } else { + return false; + } +} diff --git a/libraries/AC_WPNav/AC_WPNav_Heli.h b/libraries/AC_WPNav/AC_WPNav_Heli.h new file mode 100644 index 0000000000000..adbb63e7e5e66 --- /dev/null +++ b/libraries/AC_WPNav/AC_WPNav_Heli.h @@ -0,0 +1,98 @@ +#pragma once + +/// @file AC_WPNav_Heli.h +/// @brief ArduCopter waypoint navigation library for traditional helicopters + +#include "AC_WPNav.h" +#include +#include +#include // Mission command library + +// default values +#define WPNAV_HELI_MIN_L1_NAV_SPEED 550.0f +#define WPNAV_HELI_MIN_L1_NAV_CMD_SPEED 700.0f + +class AC_WPNav_Heli : public AC_WPNav { +public: + AC_WPNav_Heli( const AP_InertialNav& inav, + const AP_AHRS_View& ahrs, + AC_PosControl& pos_control, + const AC_AttitudeControl& attitude_control, + AP_SpdHgtControl_Heli *helispdhgtctrl, + AP_L1_Control_Heli& L1_controller); + + bool use_l1_navigation(); + + /// wp_and_spline_init - initialise straight line and spline waypoint controllers + /// updates target roll, pitch targets and I terms based on vehicle lean angles + /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination + void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}) override; + + /// set_wp_destination waypoint using location class + /// returns false if conversion from location to vector from ekf origin cannot be calculated + bool set_wp_destination_loc(const Location& destination) override; + bool set_wp_destination_next_loc(const Location& destination) override; + + /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) + /// terrain_alt should be true if destination.z is a desired altitude above terrain + bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false) override; + bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false) override; + + /// set the L1 navigation controller origin and destination + void set_L1_wp_origin_and_destination(const Vector3f& destination); + + /// update_wpnav - run the wp controller - should be called at 100hz or higher + bool update_l1_wpnav(); + + /// advance_wp_target_along_track - move target location along track from origin to destination + bool advance_l1_wp_target_along_track(float dt); + + /// reached l1 destination + bool reached_l1_destination() const { return _reached_l1_destination; } + + /// get L1 desired roll, pitch which should be fed into stabilize controllers + int32_t get_l1_roll() const { return _L1_controller.nav_roll_cd(); } + int32_t get_l1_pitch() const { return _helispdhgtctrl->get_pitch(); } + int32_t get_angle_total() const { return _L1_controller.get_angle_total(); } + + // get desired yaw rate to coordinate turns + int32_t get_yaw_rate() const { return _L1_controller.turn_rate_cds(); } + + // set stopping at waypoint + void set_stopping_at_wp(bool stopping) { _stopping_at_waypoint = stopping; } + + bool is_L1_active() const; + + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // References to external libraries + AP_L1_Control_Heli _L1_controller; + // pointer to the SpdHgtControl object + AP_SpdHgtControl_Heli *_helispdhgtctrl; + + Vector3f _prev_WP_pos; + Vector3f _this_WP_pos; + Vector3f _next_WP_pos; + Vector3f _turn_WP_pos; + + bool _in_turn; + float _turn_accept_dist; + + uint8_t _l1_loiter_type; // set L1 Nav loiter type + + +private: + + + // parameters + + // L1 controller variables + AP_Int8 _l1_nav_use; + AP_Float _loiter_radius; + bool _reached_l1_destination; + bool _stopping_at_waypoint; + uint32_t _wp_last_l1_update; + float _desired_speed; +}; diff --git a/libraries/AP_L1_Control/AP_L1_Control_Heli.cpp b/libraries/AP_L1_Control/AP_L1_Control_Heli.cpp new file mode 100644 index 0000000000000..bc277e9fe6b60 --- /dev/null +++ b/libraries/AP_L1_Control/AP_L1_Control_Heli.cpp @@ -0,0 +1,561 @@ +#include +#include "AP_L1_Control_Heli.h" + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AP_L1_Control_Heli::var_info[] = { + // @Param: PERIOD + // @DisplayName: L1 control period + // @Description: Period in seconds of L1 tracking loop. This parameter is the primary control for agressiveness of turns in auto mode. This needs to be larger for less responsive airframes. The default of 20 is quite conservative, but for most RC aircraft will lead to reasonable flight. For smaller more agile aircraft a value closer to 15 is appropriate, or even as low as 10 for some very agile aircraft. When tuning, change this value in small increments, as a value that is much too small (say 5 or 10 below the right value) can lead to very radical turns, and a risk of stalling. + // @Units: s + // @Range: 1 60 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("PERIOD", 0, AP_L1_Control_Heli, _L1_period, 20), + + // @Param: DAMPING + // @DisplayName: L1 control damping ratio + // @Description: Damping ratio for L1 control. Increase this in increments of 0.05 if you are getting overshoot in path tracking. You should not need a value below 0.7 or above 0.85. + // @Range: 0.6 1.0 + // @Increment: 0.05 + // @User: Advanced + AP_GROUPINFO("DAMPING", 1, AP_L1_Control_Heli, _L1_damping, 0.75f), + + // @Param: XTRACK_I + // @DisplayName: L1 control crosstrack integrator gain + // @Description: Crosstrack error integrator gain. This gain is applied to the crosstrack error to ensure it converges to zero. Set to zero to disable. Smaller values converge slower, higher values will cause crosstrack error oscillation. + // @Range: 0 0.1 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("XTRACK_I", 2, AP_L1_Control_Heli, _L1_xtrack_i_gain, 0.02), + + // @Param: TURN_SF + // @DisplayName: Turn Rate Scale Factor + // @Description: Turn Rate Scale Factor allow user to scale the yaw input calculated from the bank angle + // @Range: 0.2 2.0 + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("TURN_SF", 3, AP_L1_Control_Heli, _turn_rate_scale_factor, 1.0f), + + // @Param: SPEED_MIN + // @DisplayName: Minimum speed for L1 Nav + // @Description: Min speed for L1 Nav + // @Range: 5.0 20.0 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("SPEED_MIN", 4, AP_L1_Control_Heli, _speed_min, 5.0f), + + AP_GROUPEND +}; + +//Bank angle command based on angle between aircraft velocity vector and reference vector to path. +//S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," +//Proceedings of the AIAA Guidance, Navigation and Control +//Conference, Aug 2004. AIAA-2004-4900. +//Modified to use PD control for circle tracking to enable loiter radius less than L1 length +//Modified to enable period and damping of guidance loop to be set explicitly +//Modified to provide explicit control over capture angle + + +/* + Wrap AHRS yaw if in reverse - radians + */ +float AP_L1_Control_Heli::get_yaw() const +{ + if (_reverse) { + return wrap_PI(M_PI + _ahrs.yaw); + } + return _ahrs.yaw; +} + +/* + Wrap AHRS yaw sensor if in reverse - centi-degress + */ +float AP_L1_Control_Heli::get_yaw_sensor() const +{ + if (_reverse) { + return wrap_180_cd(18000 + _ahrs.yaw_sensor); + } + return _ahrs.yaw_sensor; +} + +/* + return the bank angle needed to achieve tracking from the last + update_*() operation + */ +int32_t AP_L1_Control_Heli::nav_roll_cd(void) const +{ + float ret; + ret = cosf(_ahrs.pitch)*degrees(atanf(_latAccDem * (1.0f/GRAVITY_MSS)) * 100.0f); + ret = constrain_float(ret, -9000, 9000); + return ret; +} + +/* + return the turn rate needed to achieve tracking from the last + update_*() operation + */ +int32_t AP_L1_Control_Heli::turn_rate_cds(void) const +{ + float ret; + float turn_rate_scaling = constrain_float(_turn_rate_scale_factor, 0.2, 2.0); + ret = turn_rate_scaling * (GRAVITY_MSS / _groundSpeed) * 5730.0f * sinf(radians(nav_roll_cd() * 0.01)); + ret = constrain_float(ret, -12000, 12000); + return ret; +} + +/* + return the lateral acceleration needed to achieve tracking from the last + update_*() operation + */ +float AP_L1_Control_Heli::lateral_acceleration(void) const +{ + return _latAccDem; +} + +int32_t AP_L1_Control_Heli::nav_bearing_cd(void) const +{ + return wrap_180_cd(RadiansToCentiDegrees(_nav_bearing)); +} + +int32_t AP_L1_Control_Heli::bearing_error_cd(void) const +{ + return RadiansToCentiDegrees(_bearing_error); +} + +int32_t AP_L1_Control_Heli::target_bearing_cd(void) const +{ + return wrap_180_cd(_target_bearing_cd); +} + +/* + this is the turn distance assuming a 90 degree turn + */ +float AP_L1_Control_Heli::turn_distance(float wp_radius) const +{ + wp_radius *= sq(_ahrs.get_EAS2TAS()); + return MIN(wp_radius, _L1_dist); +} + +/* + this approximates the turn distance for a given turn angle. If the + turn_angle is > 90 then a 90 degree turn distance is used, otherwise + the turn distance is reduced linearly. + This function allows straight ahead mission legs to avoid thinking + they have reached the waypoint early, which makes things like camera + trigger and ball drop at exact positions under mission control much easier + */ +float AP_L1_Control_Heli::turn_distance(float wp_radius, float turn_angle) const +{ + float distance_90 = turn_distance(wp_radius); + turn_angle = fabsf(turn_angle); + if (turn_angle >= 90) { + return distance_90; + } + return distance_90 * turn_angle / 90.0f; +} + +float AP_L1_Control_Heli::loiter_radius(const float radius) const +{ + // prevent an insane loiter bank limit + float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f); + float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS; + + float nominal_velocity_sea_level = 0.0f; + if(_spdHgtControl != nullptr) { + nominal_velocity_sea_level = _spdHgtControl->get_target_airspeed(); + } + + float eas2tas_sq = sq(_ahrs.get_EAS2TAS()); + + if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) || + is_zero(lateral_accel_sea_level)) { + // Missing a sane input for calculating the limit, or the user has + // requested a straight scaling with altitude. This will always vary + // with the current altitude, but will at least protect the airframe + return radius * eas2tas_sq; + } else { + float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level; + if (sea_level_radius > radius) { + // If we've told the plane that its sea level radius is unachievable fallback to + // straight altitude scaling + return radius * eas2tas_sq; + } else { + // select the requested radius, or the required altitude scale, whichever is safer + return MAX(sea_level_radius * eas2tas_sq, radius); + } + } +} + +bool AP_L1_Control_Heli::reached_loiter_target(void) +{ + return _WPcircle; +} + +/** + prevent indecision in our turning by using our previous turn + decision if we are in a narrow angle band pointing away from the + target and the turn angle has changed sign + */ +void AP_L1_Control_Heli::_prevent_indecision(float &Nu) +{ + const float Nu_limit = 0.9f*M_PI; + if (fabsf(Nu) > Nu_limit && + fabsf(_last_Nu) > Nu_limit && + labs(wrap_180_cd(_target_bearing_cd - get_yaw_sensor())) > 12000 && + Nu * _last_Nu < 0.0f) { + // we are moving away from the target waypoint and pointing + // away from the waypoint (not flying backwards). The sign + // of Nu has also changed, which means we are + // oscillating in our decision about which way to go + Nu = _last_Nu; + } +} + +// update L1 control for waypoint navigation +void AP_L1_Control_Heli::update_waypoint(const Vector2f &_origin, const Vector2f &_destination, float dist_min) +{ + + float Nu; + float xtrackVel; + float ltrackVel; + Vector2f current_pos; + Vector2f origin = _origin * 0.01; + Vector2f destination = _destination * 0.01; + + uint32_t now = AP_HAL::micros(); + float dt = (now - _last_update_waypoint_us) * 1.0e-6f; + if (dt > 1) { + // controller hasn't been called for an extended period of + // time. Reinitialise it. + _L1_xtrack_i = 0.0f; + } + if (dt > 0.1) { + dt = 0.1; + } + _last_update_waypoint_us = now; + + // Calculate L1 gain required for specified damping + float K_L1 = 4.0f * _L1_damping * _L1_damping; + + Vector3f curr_pos_NED; + if (!_ahrs.get_relative_position_NED_origin(curr_pos_NED)) { + _data_is_stale = true; + return; + } + current_pos = curr_pos_NED.xy(); + + Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); + + // update _target_bearing_cd + _target_bearing_cd = get_bearing_cd(current_pos, destination); + + //Calculate groundspeed + _groundSpeed = _groundspeed_vector.length(); + if (_groundSpeed < 0.1f) { + // use a small ground speed vector in the right direction, + // allowing us to use the compass heading at zero GPS velocity + _groundSpeed = 0.1f; + _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * _groundSpeed; + } + + // Calculate time varying control parameters + // Calculate the L1 length required for specified period + // 0.3183099 = 1/1/pipi + _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * _groundSpeed, dist_min); + + // Calculate the NE position of WP B relative to WP A + Vector2f AB = destination - origin; + float AB_length = AB.length(); + + // Check for AB zero length and track directly to the destination + // if too small + if (AB.length() < 1.0e-6f) { + AB = destination - current_pos; + if (AB.length() < 1.0e-6f) { + AB = Vector2f(cosf(get_yaw()), sinf(get_yaw())); + } + } + AB.normalize(); + + // Calculate the NE position of the aircraft relative to WP A + Vector2f A_air = current_pos - origin; + + // calculate distance to target track, for reporting + _crosstrack_error = A_air % AB; + + //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A + //and further than L1 distance from WP A. Then use WP A as the L1 reference point + //Otherwise do normal L1 guidance + float WP_A_dist = A_air.length(); + float alongTrackDist = A_air * AB; + if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f) + { + //Calc Nu to fly To WP A + Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft + xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line + ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line + Nu = atan2f(xtrackVel,ltrackVel); + _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point + } else if (alongTrackDist > AB_length + _groundSpeed*3) { + // we have passed point B by 3 seconds. Head towards B + // Calc Nu to fly To WP B + Vector2f B_air = current_pos - destination; + Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft + xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line + ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line + Nu = atan2f(xtrackVel,ltrackVel); + _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point + } else { //Calc Nu to fly along AB line + + //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) + xtrackVel = _groundspeed_vector % AB; // Velocity cross track + ltrackVel = _groundspeed_vector * AB; // Velocity along track + float Nu2 = atan2f(xtrackVel,ltrackVel); + //Calculate Nu1 angle (Angle to L1 reference point) + float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f); + //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg + sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); + float Nu1 = asinf(sine_Nu1); + + // compute integral error component to converge to a crosstrack of zero when traveling + // straight but reset it when disabled or if it changes. That allows for much easier + // tuning by having it re-converge each time it changes. + if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) { + _L1_xtrack_i = 0; + _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; + } else if (fabsf(Nu1) < radians(5)) { + _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; + + // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at + _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f); + } + + // to converge to zero we must push Nu1 harder + Nu1 += _L1_xtrack_i; + + Nu = Nu1 + Nu2; + _nav_bearing = wrap_PI(atan2f(AB.y, AB.x) + Nu1); // bearing (radians) from AC to L1 point + } + + _prevent_indecision(Nu); + _last_Nu = Nu; + + //Limit Nu to +-(pi/2) + Nu = constrain_float(Nu, -1.5708f, +1.5708f); + _latAccDem = K_L1 * _groundSpeed * _groundSpeed / _L1_dist * sinf(Nu); + + // Waypoint capture status is always false during waypoint following + _WPcircle = false; + + _bearing_error = Nu; // bearing error angle (radians), +ve to left of track + + _data_is_stale = false; // status are correctly updated with current waypoint data +} + +// update L1 control for loitering +void AP_L1_Control_Heli::update_loiter(const Vector2f &_center_pos, float radius, int8_t loiter_direction) +{ + Vector2f center_pos = _center_pos * 0.01f; //convert to meters + Vector2f current_pos; + // scale loiter radius with square of EAS2TAS to allow us to stay + // stable at high altitude + radius = loiter_radius(fabsf(radius)); + + // Calculate guidance gains used by PD loop (used during circle tracking) + float omega = (6.2832f / _L1_period); + float Kx = omega * omega; + float Kv = 2.0f * _L1_damping * omega; + + // Calculate L1 gain required for specified damping (used during waypoint capture) + float K_L1 = 4.0f * _L1_damping * _L1_damping; + + Vector3f curr_pos_NED; + if (!_ahrs.get_relative_position_NED_origin(curr_pos_NED)) { + _data_is_stale = true; + return; + } + current_pos = curr_pos_NED.xy(); + + Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); + + //Calculate groundspeed + _groundSpeed = MAX(_groundspeed_vector.length() , 1.0f); + + + // update _target_bearing_cd + _target_bearing_cd = get_bearing_cd(current_pos, center_pos); + + + // Calculate time varying control parameters + // Calculate the L1 length required for specified period + // 0.3183099 = 1/pi + _L1_dist = 0.3183099f * _L1_damping * _L1_period * _groundSpeed; + + //Calculate the NE position of the aircraft relative to WP A + Vector2f A_air = current_pos - center_pos; + + // Calculate the unit vector from WP A to aircraft + // protect against being on the waypoint and having zero velocity + // if too close to the waypoint, use the velocity vector + // if the velocity vector is too small, use the heading vector + Vector2f A_air_unit; + if (A_air.length() > 0.1f) { + A_air_unit = A_air.normalized(); + } else { + if (_groundspeed_vector.length() < 0.1f) { + A_air_unit = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)); + } else { + A_air_unit = _groundspeed_vector.normalized(); + } + } + + //Calculate Nu to capture center_WP + float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP + float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP + float Nu = atan2f(xtrackVelCap,ltrackVelCap); + + _prevent_indecision(Nu); + _last_Nu = Nu; + + Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2 + + //Calculate lat accln demand to capture center_WP (use L1 guidance law) + float latAccDemCap = K_L1 * _groundSpeed * _groundSpeed / _L1_dist * sinf(Nu); + + //Calculate radial position and velocity errors + float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity + float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle + + // keep crosstrack error for reporting + _crosstrack_error = xtrackErrCirc; + + //Calculate PD control correction to circle waypoint_ahrs.roll + float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv); + + //Calculate tangential velocity + float velTangent = xtrackVelCap * float(loiter_direction); + + //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way + if (ltrackVelCap < 0.0f && velTangent < 0.0f) { + latAccDemCircPD = MAX(latAccDemCircPD, 0.0f); + } + + // Calculate centripetal acceleration demand + float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc)); + + //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand + float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); + + // Perform switchover between 'capture' and 'circle' modes at the + // point where the commands cross over to achieve a seamless transfer + // Only fly 'capture' mode if outside the circle + if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) { + _latAccDem = latAccDemCap; + _WPcircle = false; + _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track + _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point + } else { + _latAccDem = latAccDemCirc; + _WPcircle = true; + _bearing_error = 0.0f; // bearing error (radians), +ve to left of track + _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point + } + + _data_is_stale = false; // status are correctly updated with current waypoint data +} + + +// update L1 control for heading hold navigation +void AP_L1_Control_Heli::update_heading_hold(int32_t navigation_heading_cd) +{ + // Calculate normalised frequency for tracking loop + const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period + // Calculate additional damping gain + + int32_t Nu_cd; + float Nu; + + // copy to _target_bearing_cd and _nav_bearing + _target_bearing_cd = wrap_180_cd(navigation_heading_cd); + _nav_bearing = radians(navigation_heading_cd * 0.01f); + + Nu_cd = _target_bearing_cd - wrap_180_cd(_ahrs.yaw_sensor); + Nu_cd = wrap_180_cd(Nu_cd); + Nu = radians(Nu_cd * 0.01f); + + Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); + + //Calculate groundspeed + _groundSpeed = _groundspeed_vector.length(); + + // Calculate time varying control parameters + _L1_dist = _groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency + float VomegaA = _groundSpeed * omegaA; + + // Waypoint capture status is always false during heading hold + _WPcircle = false; + + _crosstrack_error = 0; + + _bearing_error = Nu; // bearing error angle (radians), +ve to left of track + + // Limit Nu to +-pi + Nu = constrain_float(Nu, -M_PI_2, M_PI_2); + _latAccDem = 2.0f*sinf(Nu)*VomegaA; + + _data_is_stale = false; // status are correctly updated with current waypoint data +} + +// update L1 control for level flight on current heading +void AP_L1_Control_Heli::update_level_flight(void) +{ + // copy to _target_bearing_cd and _nav_bearing + _target_bearing_cd = _ahrs.yaw_sensor; + _nav_bearing = _ahrs.yaw; + _bearing_error = 0; + _crosstrack_error = 0; + + // Waypoint capture status is always false during heading hold + _WPcircle = false; + + _latAccDem = 0; + + _data_is_stale = false; // status are correctly updated with current waypoint data +} + +/* + reset the total loiter angle + */ +void AP_L1_Control_Heli::loiter_angle_reset(void) +{ + loiter.sum_cd = 0; + loiter.total_cd = 0; +} + +/* + update the total angle we have covered in a loiter. Used to support + commands to do N circles of loiter + */ +void AP_L1_Control_Heli::loiter_angle_update(void) +{ +// int32_t target_bearing_cd = wrap_180_cd(_target_bearing_cd); + int32_t loiter_delta_cd; + + if (loiter.sum_cd == 0 && !reached_loiter_target()) { + // we don't start summing until we are doing the real loiter + loiter_delta_cd = 0; + } else if (loiter.sum_cd == 0) { + // use 1 cd for initial delta + loiter_delta_cd = 1; + } else { + loiter_delta_cd = wrap_180_cd(_target_bearing_cd) - loiter.old_target_bearing_cd; + } + + loiter.old_target_bearing_cd = wrap_180_cd(_target_bearing_cd); + loiter_delta_cd = wrap_180_cd(loiter_delta_cd); + loiter.sum_cd += loiter_delta_cd * loiter.direction; + +} + diff --git a/libraries/AP_L1_Control/AP_L1_Control_Heli.h b/libraries/AP_L1_Control/AP_L1_Control_Heli.h new file mode 100644 index 0000000000000..f617affa54bc8 --- /dev/null +++ b/libraries/AP_L1_Control/AP_L1_Control_Heli.h @@ -0,0 +1,167 @@ +#pragma once + +/// @file AP_L1_Control_Heli.h +/// @brief L1 Control algorithm. This is a instance of an +/// AP_Navigation class + +/* + * Originally written by Brandon Jones 2013 + * + * Modified by Paul Riseborough 2013 to provide: + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + */ + +#include +#include +#include +#include +#include + +class AP_L1_Control_Heli { +public: + AP_L1_Control_Heli(AP_AHRS &ahrs, const AP_SpdHgtControl_Heli *spdHgtControl) + : _ahrs(ahrs) + , _spdHgtControl(spdHgtControl) + { + AP_Param::setup_object_defaults(this, var_info); + } + + /* see AP_Navigation.h for the definitions and units of these + * functions */ + int32_t nav_roll_cd(void) const; + float lateral_acceleration(void) const; + + // return the turn rate needed to achieve tracking from the last update_*() operation + int32_t turn_rate_cds(void) const; + + // return the desired track heading angle(centi-degrees) + int32_t nav_bearing_cd(void) const; + + // return the heading error angle (centi-degrees) +ve to left of track + int32_t bearing_error_cd(void) const; + + float crosstrack_error(void) const { return _crosstrack_error; } + float crosstrack_error_integrator(void) const { return _L1_xtrack_i; } + + int32_t target_bearing_cd(void) const; + float turn_distance(float wp_radius) const; + float turn_distance(float wp_radius, float turn_angle) const; + float loiter_radius (const float loiter_radius) const; + void update_waypoint(const Vector2f &prev_WP, const Vector2f &next_WP, float dist_min = 0.0f); + void update_loiter(const Vector2f ¢er_WP, float radius, int8_t loiter_direction); + void update_heading_hold(int32_t navigation_heading_cd); + void update_level_flight(void); + bool reached_loiter_target(void); + void loiter_angle_reset(void); + void loiter_angle_update(void); + + // get the total angle traveled while in loiter + int32_t get_angle_total() const { return loiter.sum_cd; } + + + // set the default NAVL1_PERIOD + void set_default_period(float period) { + _L1_period.set_default(period); + } + + void set_data_is_stale(void) { + _data_is_stale = true; + } + bool data_is_stale(void) const { + return _data_is_stale; + } + + // this supports the NAVl1_* user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + + void set_reverse(bool reverse) { + _reverse = reverse; + } + +private: + // reference to the AHRS object + AP_AHRS &_ahrs; + + // pointer to the SpdHgtControl object + const AP_SpdHgtControl_Heli *_spdHgtControl; + + // lateral acceration in m/s required to fly to the + // L1 reference point (+ve to right) + float _latAccDem; + + // L1 tracking distance in meters which is dynamically updated + float _L1_dist; + + // Status which is true when the vehicle has started circling the WP + bool _WPcircle; + + // bearing angle (radians) to L1 point + float _nav_bearing; + + // bearing error angle (radians) +ve to left of track + float _bearing_error; + + // crosstrack error in meters + float _crosstrack_error; + + // target bearing in centi-degrees from last update + int32_t _target_bearing_cd; + + // L1 tracking loop period (sec) + AP_Float _L1_period; + // L1 tracking loop damping ratio + AP_Float _L1_damping; + // turn rate scale factor + AP_Float _turn_rate_scale_factor; + // minimum speed for L1 Nav + AP_Float _speed_min; + + // previous value of cross-track velocity + float _last_Nu; + + // prevent indecision in waypoint tracking + void _prevent_indecision(float &Nu); + + // integral feedback to correct crosstrack error. Used to ensure xtrack converges to zero. + // For tuning purposes it's helpful to clear the integrator when it changes so a _prev is used + float _L1_xtrack_i = 0; + AP_Float _L1_xtrack_i_gain; + float _L1_xtrack_i_gain_prev = 0; + uint32_t _last_update_waypoint_us; + bool _data_is_stale = true; + + float _groundSpeed; + + float _loiter_bank_limit; + + bool _reverse = false; + float get_yaw() const; + float get_yaw_sensor() const; + float get_speed_min() { return _speed_min; } + + /* + meta data to support counting the number of circles in a loiter + */ + struct { + // previous target bearing, used to update sum_cd + int32_t old_target_bearing_cd; + + // Total desired rotation in a loiter. Used for Loiter Turns commands. + int32_t total_cd; + + // total angle completed in the loiter so far + int32_t sum_cd; + + // Direction for loiter. 1 for clockwise, -1 for counter-clockwise + int8_t direction = 1; + + // start time of the loiter. Milliseconds. + uint32_t start_time_ms; + + // The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds. + uint32_t time_max_ms; + } loiter; + +}; diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.cpp b/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.cpp new file mode 100644 index 0000000000000..55ba9d85219c9 --- /dev/null +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.cpp @@ -0,0 +1,160 @@ +#include "AP_SpdHgtControl_Heli.h" +#include + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AP_SpdHgtControl_Heli::var_info[] = { + + // @Param: VEL_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + + // @Param: VEL_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: VEL_D + // @DisplayName: Velocity (horizontal) D gain + // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity + // @Range: 0.00 1.00 + // @Increment: 0.001 + // @User: Advanced + + // @Param: VEL_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: cm/s/s + // @User: Advanced + + // @Param: VEL_FILT + // @DisplayName: Velocity (horizontal) input filter + // @Description: Velocity (horizontal) input filter. This filter (in hz) is applied to the input for P and I terms + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + + // @Param: VEL_FF + // @DisplayName: Velocity (horizontal) input filter + // @Description: Velocity (horizontal) input filter. This filter (in hz) is applied to the input for P and I terms + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + AP_SUBGROUPINFO(_pid_vel, "VEL_", 6, AP_SpdHgtControl_Heli, AC_PID), + + AP_GROUPEND +}; + + + +// reset speed controller +void AP_SpdHgtControl_Heli::init_controller(void) +{ + + _pid_vel.reset_I(); + accel_target = 0.0f; + Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); + _cmd_vel = 100.0f * (_groundspeed_vector.x*_ahrs.cos_yaw() + _groundspeed_vector.y*_ahrs.sin_yaw()); + _pid_vel.update_all(_cmd_vel, _cmd_vel, false); + _accel_out_last = _pid_vel.get_ff(); + +} + +// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) +void AP_SpdHgtControl_Heli::set_dt(float delta_sec) +{ + _dt = delta_sec; + + // update PID controller dt + _pid_vel.set_dt(_dt); + +} + +// update speed controller +void AP_SpdHgtControl_Heli::update_speed_controller(void) +{ + + float speed_forward, vel_p, vel_i, vel_d, vel_ff; + + Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); + + speed_forward = _groundspeed_vector.x*_ahrs.cos_yaw() + _groundspeed_vector.y*_ahrs.sin_yaw(); + + delta_speed_fwd = speed_forward - _speed_forward_last; + _speed_forward_last = speed_forward; + + // calculate velocity error + if (_cmd_vel < _vel_target) { + _cmd_vel += _accel_max * _dt; + if (_cmd_vel > _vel_target) { + _cmd_vel = _vel_target; + } + } else { + _cmd_vel -= _accel_max * _dt; + if (_cmd_vel < _vel_target) { + _cmd_vel = _vel_target; + } + } + +// _vel_error = _cmd_vel - speed_forward * 100.0f; + + // call pid controller + _pid_vel.update_all(_cmd_vel, speed_forward * 100.0f, false); + + // get p + vel_p = _pid_vel.get_p(); + + // update i term if we have not hit the accel or throttle limits OR the i term will reduce + // TODO: move limit handling into the PI and PID controller +// if (!_limit.accel_xy && !_motors.limit.throttle_upper) { + vel_i = _pid_vel.get_i(); +// } else { +// vel_i = _pid_vel.get_i_shrink(); +// } + + // get d + vel_d = _pid_vel.get_d(); + + // get ff + vel_ff = _pid_vel.get_ff(); + + accel_target = vel_ff + vel_p + vel_i + vel_d; + + // filter correction acceleration + _accel_target_filter.set_cutoff_frequency(10.0f); + _accel_target_filter.apply(accel_target, _dt); + + // the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles + + if (accel_target > _accel_out_last + _accel_max) { + accel_target = _accel_out_last + _accel_max; + } else if (accel_target < _accel_out_last - _accel_max) { + accel_target = _accel_out_last - _accel_max; + } + + if (fabsf(delta_speed_fwd * 100.0f) > _accel_max * _dt) { + _flag_limit_accel = true; + } else if (fabsf(delta_speed_fwd * 100.0f) < _accel_max * _dt){ + _flag_limit_accel = false; + } + + float accel_out = 0.0f; + if ((_flag_limit_accel && fabsf(accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { + accel_out = accel_target; + } else { + accel_out = _accel_out_last; + } + _accel_out_last = accel_out; + + // update angle targets that will be passed to stabilize controller + _pitch_target = atanf(-accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); + +} diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.h new file mode 100644 index 0000000000000..7fd226af2e2db --- /dev/null +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl_Heli.h @@ -0,0 +1,87 @@ +/// @file AP_SpdHgtControl_Heli.h +/// @brief Helicopter Speed & Height Control. This is a instance of an +/// AP_SpdHgtControl class + + +#pragma once + +#include +#include +#include +#include +#include // PID library + +#define AP_SPDHGTCTRL_VEL_P 1.0f +#define AP_SPDHGTCTRL_VEL_I 0.5f +#define AP_SPDHGTCTRL_VEL_D 0.0f +#define AP_SPDHGTCTRL_VEL_IMAX 1000.0f +#define AP_SPDHGTCTRL_VEL_FF 0.15f +#define AP_SPDHGTCTRL_VEL_FILT_HZ 10.0f +#define AP_SPDHGTCTRL_VEL_DT 0.0025f + +class AP_SpdHgtControl_Heli { +public: + AP_SpdHgtControl_Heli(AP_AHRS &ahrs) + : _ahrs(ahrs) + , _pid_vel(AP_SPDHGTCTRL_VEL_P, AP_SPDHGTCTRL_VEL_I, AP_SPDHGTCTRL_VEL_D, AP_SPDHGTCTRL_VEL_FF, AP_SPDHGTCTRL_VEL_IMAX, AP_SPDHGTCTRL_VEL_FILT_HZ, AP_SPDHGTCTRL_VEL_FILT_HZ, AP_SPDHGTCTRL_VEL_FILT_HZ, AP_SPDHGTCTRL_VEL_DT) + { + AP_Param::setup_object_defaults(this, var_info); + } + + // reset controller + void init_controller(void); + + // set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) + void set_dt(float delta_sec); + + // update speed controller for use in heli wp nav + void update_speed_controller(void); + + // set_desired_speed - set speed for controller + void set_desired_speed(float speed) { _vel_target = speed; }; + + // get_pitch_target + int32_t get_pitch(void) const { return (int32_t)_pitch_target; }; + + AC_PID& get_vel_pid() { return _pid_vel; }; + + // set max accel + void set_max_accel(float accel_max) { _accel_max = accel_max; }; + + // get target accel + float get_accel_target(void) { return accel_target; }; + + // return current target airspeed + float get_target_airspeed(void) const { + return 0.0f; + } + + // get delta speed forward + float get_delta_speed(void) { return delta_speed_fwd; }; + + // this supports the user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // reference to the AHRS object + AP_AHRS &_ahrs; + + AC_PID _pid_vel; + float _vel_target; + float _pitch_target; + float _vel_error; + float _accel_max; + float _speed_forward_last; + bool _flag_limit_accel; + float _accel_limit_value; + float _accel_out_last; + float _cmd_vel; + float accel_target; + float delta_speed_fwd; + float _dt; + + + LowPassFilterFloat _accel_target_filter; // acceleration target filter + +};