diff options
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 415 |
1 files changed, 174 insertions, 241 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cfa98e573..975ac85ff 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -745,312 +745,245 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_parameters.speed_weight); - /* execute navigation once we have a setpoint */ - if (_setpoint_valid && _control_mode.flag_control_auto_enabled) { + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + /* current waypoint (the one currently heading for) */ + math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + /* previous waypoint */ + math::Vector2f prev_wp; - /* previous waypoint */ - math::Vector2f prev_wp; - - if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); - - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); - - } - - // XXX add RTL switch - if (mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { - - math::Vector2f rtl_pos(_launch_lat, _launch_lon); - - _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (mission_item_triplet.previous_valid) { + prev_wp.setX(mission_item_triplet.previous.lat); + prev_wp.setY(mission_item_triplet.previous.lon); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(mission_item_triplet.current.lat); + prev_wp.setY(mission_item_triplet.current.lon); - // XXX handle case when having arrived at home (loiter) + } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, + mission_item_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - /* Horizontal landing control */ - /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - const float heading_hold_distance = 15.0f; - if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { + /* Horizontal landing control */ + /* switch to heading hold for the last meters, continue heading hold after */ + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { - /* heading hold, along the line connecting this and the last waypoint */ - + /* heading hold, along the line connecting this and the last waypoint */ + - // if (mission_item_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); - // } else { + // if (mission_item_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { - if (!land_noreturn_horizontal) //set target_bearing in first occurrence - target_bearing = _att.yaw; - //} + if (!land_noreturn_horizontal) //set target_bearing in first occurrence + target_bearing = _att.yaw; + //} // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn_horizontal = true; + land_noreturn_horizontal = true; - } else { + } else { - /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - } + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + } - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - /* Vertical landing control */ - //xxx: using the tecs altitude controller for slope control for now + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { // altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; // } - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - // XXX this could make a great param - - float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; - - float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = _parameters.land_flare_alt_relative; - float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = _parameters.land_H1_virt; - float H0 = flare_relative_alt + H1; - float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); - float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); - float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); - float horizontal_slope_displacement = (flare_length - d1); - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - - /* land with minimal speed */ + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param + + float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; + + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = _parameters.land_H1_virt; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + + if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + + /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ // _tecs.set_speed_weight(2.0f); - /* kill the throttle if param requests it */ - throttle_max = _parameters.throttle_max; - - if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - if (!land_motor_lim) { - land_motor_lim = true; - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); - } - - } - - float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + /* kill the throttle if param requests it */ + throttle_max = _parameters.throttle_max; - /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) - { - flare_curve_alt = mission_item_triplet.current.altitude; - land_stayonground = true; + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_angle_rad, math::radians(15.0f)); - - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - if (!land_noreturn_vertical) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); - land_noreturn_vertical = true; - } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + } - flare_curve_alt_last = flare_curve_alt; + float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; - } else if (wp_distance < L_wp_distance) { + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = mission_item_triplet.current.altitude; + land_stayonground = true; + } - /* minimize speed to approach speed, stay on landing slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + flare_angle_rad, math::radians(15.0f)); + + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - if (!land_onslope) { + flare_curve_alt_last = flare_curve_alt; - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); - land_onslope = true; - } + } else if (wp_distance < L_wp_distance) { - } else { + /* minimize speed to approach speed, stay on landing slope */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - /* intersect glide slope: - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope - * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { - /* stay on slope */ - altitude_desired = landing_slope_alt_desired; - //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else { - /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); - //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); - } + if (!land_onslope) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { - - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + } else { + /* intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ + altitude_desired = landing_slope_alt_desired; + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* continue horizontally */ + altitude_desired = math::max(_global_pos.alt, L_altitude); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } - } - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - - } else if (_control_mode.flag_armed) { - - /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ - - // XXX rework with smarter state machine - - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt + 25.0f; - _loiter_hold = true; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - altitude_error = _loiter_hold_alt - _global_pos.alt; + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); - - /* loiter around current position */ - _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, - 1, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 3); + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 15.0f) { - float min_pitch; + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - if (climb_out) { - min_pitch = math::radians(20.0f); + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - min_pitch = math::radians(_parameters.pitch_limit_min); - } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, min_pitch, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - if (climb_out) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } } + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + /* reset land state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn_horizontal = false; |