mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-19 14:23:57 -04:00
Plane: update for new AP_Navigation controller class
this switches ArduPlane over to use the L1 navigation controller, via a generic nav_controller object pointer. Note that the nav_controller controls all types of navigation now, including level flight and heading hold. This provides a cleaner abstraction than the old method of special case navigation handling Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell
parent
a3c2851120
commit
11eb0cfce1
@ -57,6 +57,9 @@
|
||||
#include <APM_Control.h>
|
||||
#endif
|
||||
|
||||
#include <AP_Navigation.h>
|
||||
#include <AP_L1_Control.h>
|
||||
|
||||
// Pre-AP_HAL compatibility
|
||||
#include "compat.h"
|
||||
|
||||
@ -216,6 +219,8 @@ AP_AHRS_HIL ahrs(&ins, g_gps);
|
||||
AP_AHRS_DCM ahrs(&ins, g_gps);
|
||||
#endif
|
||||
|
||||
static AP_L1_Control L1_controller(&ahrs);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
SITL sitl;
|
||||
#endif
|
||||
@ -230,6 +235,9 @@ static bool training_manual_pitch; // user has manual pitch control
|
||||
GCS_MAVLINK gcs0;
|
||||
GCS_MAVLINK gcs3;
|
||||
|
||||
// selected navigation controller
|
||||
static AP_Navigation *nav_controller = &L1_controller;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Analog Inputs
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -352,21 +360,10 @@ static bool have_position;
|
||||
// Constants
|
||||
const float radius_of_earth = 6378100; // meters
|
||||
|
||||
// This is the currently calculated direction to fly.
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t nav_bearing_cd;
|
||||
|
||||
// This is the direction to the next waypoint or loiter center
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t target_bearing_cd;
|
||||
|
||||
//This is the direction from the last waypoint to the next waypoint
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t crosstrack_bearing_cd;
|
||||
|
||||
// Direction held during phases of takeoff and landing
|
||||
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
|
||||
static int32_t hold_course = -1; // deg * 100 dir of plane
|
||||
// this is a 0..36000 value, or -1 for disabled
|
||||
static int32_t hold_course_cd = -1; // deg * 100 dir of plane
|
||||
|
||||
// There may be two active commands in Auto mode.
|
||||
// This indicates the active navigation command by index number
|
||||
@ -412,18 +409,9 @@ static uint8_t receiver_rssi;
|
||||
// The amount current ground speed is below min ground speed. Centimeters per second
|
||||
static int32_t groundspeed_undershoot = 0;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Location Errors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Difference between current bearing and desired bearing. Hundredths of a degree
|
||||
static int32_t bearing_error_cd;
|
||||
|
||||
// Difference between current altitude and desired altitude. Centimeters
|
||||
static int32_t altitude_error_cm;
|
||||
|
||||
// Distance perpandicular to the course line that we are off trackline. Meters
|
||||
static float crosstrack_error;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery Sensors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -467,21 +455,10 @@ static bool throttle_suppressed;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Loiter management
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
|
||||
static int32_t old_target_bearing_cd;
|
||||
|
||||
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
|
||||
static int32_t loiter_total;
|
||||
|
||||
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
|
||||
static int8_t loiter_direction = 1;
|
||||
|
||||
// The amount in degrees we have turned since recording old_target_bearing
|
||||
static int16_t loiter_delta;
|
||||
|
||||
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
|
||||
static int32_t loiter_sum;
|
||||
|
||||
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
|
||||
static uint32_t loiter_time_ms;
|
||||
|
||||
@ -507,6 +484,18 @@ uint32_t wp_distance;
|
||||
// Distance between previous and next waypoint. Meters
|
||||
static uint32_t wp_totalDistance;
|
||||
|
||||
/*
|
||||
meta data to support counting the number of circles in a loiter
|
||||
*/
|
||||
static struct {
|
||||
int32_t old_target_bearing_cd;
|
||||
int32_t loiter_sum_cd;
|
||||
|
||||
// Total desired rotation in a loiter. Used for Loiter Turns commands.
|
||||
int32_t loiter_total_cd;
|
||||
} loiter;
|
||||
|
||||
|
||||
// event control state
|
||||
enum event_type {
|
||||
EVENT_TYPE_RELAY=0,
|
||||
@ -758,9 +747,6 @@ static void fast_loop()
|
||||
|
||||
ahrs.update();
|
||||
|
||||
// uses the yaw from the DCM to give more accurate turns
|
||||
calc_bearing_error();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||
Log_Write_Attitude();
|
||||
|
||||
@ -1030,7 +1016,7 @@ static void update_current_flight_mode(void)
|
||||
|
||||
switch(nav_command_ID) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
if (hold_course != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) {
|
||||
if (hold_course_cd != -1 && g.rudder_steer == 0 && g.takeoff_heading_hold != 0) {
|
||||
calc_nav_roll();
|
||||
} else {
|
||||
nav_roll_cd = 0;
|
||||
@ -1092,7 +1078,7 @@ static void update_current_flight_mode(void)
|
||||
default:
|
||||
// we are doing normal AUTO flight, the special cases
|
||||
// are for takeoff and landing
|
||||
hold_course = -1;
|
||||
hold_course_cd = -1;
|
||||
land_complete = false;
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
@ -1101,7 +1087,7 @@ static void update_current_flight_mode(void)
|
||||
}
|
||||
}else{
|
||||
// hold_course is only used in takeoff and landing
|
||||
hold_course = -1;
|
||||
hold_course_cd = -1;
|
||||
|
||||
switch(control_mode) {
|
||||
case RTL:
|
||||
@ -1246,7 +1232,6 @@ static void update_navigation()
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
update_loiter();
|
||||
calc_bearing_error();
|
||||
break;
|
||||
|
||||
case MANUAL:
|
||||
|
||||
@ -325,8 +325,9 @@ static void calc_throttle()
|
||||
// ----------------------------------------------------------------------
|
||||
static void calc_nav_yaw(float speed_scaler, float ch4_inf)
|
||||
{
|
||||
if (hold_course != -1) {
|
||||
if (hold_course_cd != -1) {
|
||||
// steering on or close to ground
|
||||
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
|
||||
g.channel_rudder.servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) +
|
||||
g.kff_rudder_mix * g.channel_roll.servo_out;
|
||||
g.channel_rudder.servo_out = constrain_int16(g.channel_rudder.servo_out, -4500, 4500);
|
||||
@ -364,35 +365,7 @@ static void calc_nav_pitch()
|
||||
|
||||
static void calc_nav_roll()
|
||||
{
|
||||
#define NAV_ROLL_BY_RATE 0
|
||||
#if NAV_ROLL_BY_RATE
|
||||
// Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
|
||||
// desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
|
||||
// to turn at 15 degrees per second.
|
||||
float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);
|
||||
|
||||
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
|
||||
float speed;
|
||||
if (!ahrs.airspeed_estimate(&speed)) {
|
||||
speed = g.airspeed_cruise_cm*0.01;
|
||||
|
||||
// Floor the speed so that the user can't enter a bad value
|
||||
if(speed < 6) {
|
||||
speed = 6;
|
||||
}
|
||||
}
|
||||
|
||||
// Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
|
||||
nav_roll = ToDeg(atanf(speed*turn_rate/GRAVITY_MSS)*100);
|
||||
|
||||
#else
|
||||
// this is the old nav_roll calculation. We will use this for 2.50
|
||||
// then remove for a future release
|
||||
float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
|
||||
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
|
||||
nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
|
||||
#endif
|
||||
|
||||
nav_roll_cd = nav_controller->nav_roll_cd();
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
|
||||
}
|
||||
|
||||
|
||||
@ -271,17 +271,16 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
||||
|
||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
int16_t bearing = (hold_course==-1 ? nav_bearing_cd : hold_course) / 100;
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
nav_roll_cd * 0.01,
|
||||
nav_pitch_cd * 0.01,
|
||||
bearing,
|
||||
target_bearing_cd * 0.01,
|
||||
nav_controller->nav_bearing_cd() * 0.01f,
|
||||
nav_controller->target_bearing_cd() * 0.01f,
|
||||
wp_distance,
|
||||
altitude_error_cm * 0.01,
|
||||
airspeed_error_cm,
|
||||
crosstrack_error);
|
||||
nav_controller->crosstrack_error());
|
||||
}
|
||||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
|
||||
@ -489,8 +489,8 @@ static void Log_Write_Nav_Tuning()
|
||||
LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
|
||||
yaw : (uint16_t)ahrs.yaw_sensor,
|
||||
wp_distance : wp_distance,
|
||||
target_bearing_cd : (uint16_t)target_bearing_cd,
|
||||
nav_bearing_cd : (uint16_t)nav_bearing_cd,
|
||||
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
|
||||
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
|
||||
altitude_error_cm : (int16_t)altitude_error_cm,
|
||||
airspeed_cm : (int16_t)airspeed.get_airspeed_cm()
|
||||
};
|
||||
|
||||
@ -73,7 +73,7 @@ public:
|
||||
k_param_reset_mission_chan,
|
||||
k_param_land_flare_alt,
|
||||
k_param_land_flare_sec,
|
||||
k_param_crosstrack_min_distance,
|
||||
k_param_crosstrack_min_distance, // unused
|
||||
k_param_rudder_steer,
|
||||
k_param_throttle_nudge,
|
||||
k_param_alt_offset,
|
||||
@ -83,6 +83,7 @@ public:
|
||||
k_param_takeoff_heading_hold,
|
||||
k_param_hil_servos,
|
||||
k_param_vtail_output,
|
||||
k_param_nav_controller,
|
||||
|
||||
// 110: Telemetry control
|
||||
//
|
||||
@ -125,8 +126,8 @@ public:
|
||||
//
|
||||
// 150: Navigation parameters
|
||||
//
|
||||
k_param_crosstrack_gain = 150,
|
||||
k_param_crosstrack_entry_angle,
|
||||
k_param_crosstrack_gain = 150, // unused
|
||||
k_param_crosstrack_entry_angle, // unused
|
||||
k_param_roll_limit_cd,
|
||||
k_param_pitch_limit_max_cd,
|
||||
k_param_pitch_limit_min_cd,
|
||||
@ -134,7 +135,7 @@ public:
|
||||
k_param_RTL_altitude_cm,
|
||||
k_param_inverted_flight_ch,
|
||||
k_param_min_gndspeed_cm,
|
||||
k_param_crosstrack_use_wind,
|
||||
k_param_crosstrack_use_wind, // unused
|
||||
|
||||
|
||||
//
|
||||
@ -219,6 +220,7 @@ public:
|
||||
k_param_rollController,
|
||||
k_param_pitchController,
|
||||
k_param_yawController,
|
||||
k_param_L1_controller,
|
||||
|
||||
//
|
||||
// 240: PID Controllers
|
||||
@ -255,12 +257,8 @@ public:
|
||||
// speed used for speed scaling
|
||||
AP_Float scaling_speed;
|
||||
|
||||
// Crosstrack navigation
|
||||
//
|
||||
AP_Float crosstrack_gain;
|
||||
AP_Int16 crosstrack_entry_angle;
|
||||
AP_Int8 crosstrack_use_wind;
|
||||
AP_Int16 crosstrack_min_distance;
|
||||
// navigation controller type. See AP_Navigation::ControllerType
|
||||
AP_Int8 nav_controller;
|
||||
|
||||
// Estimation
|
||||
//
|
||||
|
||||
@ -143,38 +143,12 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: Advanced
|
||||
GSCALAR(land_flare_sec, "LAND_FLARE_SEC", 2.0),
|
||||
|
||||
// @Param: XTRK_GAIN_SC
|
||||
// @DisplayName: Crosstrack Gain
|
||||
// @Description: The scale between distance off the line and angle to meet the line (in Degrees * 100)
|
||||
// @Range: 0 2000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", XTRACK_GAIN_SCALED),
|
||||
|
||||
// @Param: XTRK_ANGLE_CD
|
||||
// @DisplayName: Crosstrack Entry Angle
|
||||
// @Description: Maximum angle used to correct for track following.
|
||||
// @Units: centi-Degrees
|
||||
// @Range: 0 9000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE),
|
||||
|
||||
// @Param: XTRK_USE_WIND
|
||||
// @DisplayName: Crosstrack Wind correction
|
||||
// @Description: If enabled, use wind estimation for navigation crosstrack when using a compass for yaw
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND", 1),
|
||||
|
||||
// @Param: XTRK_MIN_DIST
|
||||
// @DisplayName: Crosstrack mininum distance
|
||||
// @Description: Minimum distance in meters between waypoints to do crosstrack correction.
|
||||
// @Units: Meters
|
||||
// @Range: 0 32767
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", 50),
|
||||
// @Param: NAV_CONTROLLER
|
||||
// @DisplayName: Navigation controller selection
|
||||
// @Description: Which navigation controller to enable
|
||||
// @Values: 0:Legacy,1:L1Controller
|
||||
// @User: Standard
|
||||
GSCALAR(nav_controller, "NAV_CONTROLLER", AP_Navigation::CONTROLLER_L1),
|
||||
|
||||
// @Param: ALT_MIX
|
||||
// @DisplayName: Gps to Baro Mix
|
||||
@ -208,7 +182,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @DisplayName: Waypoint Radius
|
||||
// @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit.
|
||||
// @Units: Meters
|
||||
// @Range: 1 127
|
||||
// @Range: 1 32767
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT),
|
||||
@ -736,6 +710,10 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
|
||||
GOBJECT(airspeed, "ARSPD_", AP_Airspeed),
|
||||
|
||||
// @Group: NAVL1_
|
||||
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
|
||||
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
// @Group: MNT_
|
||||
// @Path: ../libraries/AP_Mount/AP_Mount.cpp
|
||||
|
||||
@ -193,23 +193,13 @@ static void set_next_WP(const struct Location *wp)
|
||||
}
|
||||
|
||||
// zero out our loiter vals to watch for missed waypoints
|
||||
loiter_delta = 0;
|
||||
loiter_sum = 0;
|
||||
loiter_total = 0;
|
||||
loiter_angle_reset();
|
||||
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
loiter_angle_reset();
|
||||
}
|
||||
|
||||
static void set_guided_WP(void)
|
||||
@ -230,15 +220,8 @@ static void set_guided_WP(void)
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
loiter_angle_reset();
|
||||
}
|
||||
|
||||
// run this at setup on the ground
|
||||
|
||||
@ -281,7 +281,7 @@ static void do_loiter_unlimited()
|
||||
static void do_loiter_turns()
|
||||
{
|
||||
set_next_WP(&next_nav_command);
|
||||
loiter_total = next_nav_command.p1 * 360;
|
||||
loiter.loiter_total_cd = next_nav_command.p1 * 36000UL;
|
||||
loiter_set_direction_wp(&next_nav_command);
|
||||
}
|
||||
|
||||
@ -299,24 +299,24 @@ static void do_loiter_time()
|
||||
static bool verify_takeoff()
|
||||
{
|
||||
if (ahrs.yaw_initialised()) {
|
||||
if (hold_course == -1 && g.takeoff_heading_hold != 0) {
|
||||
if (hold_course_cd == -1 && g.takeoff_heading_hold != 0) {
|
||||
// save our current course to take off
|
||||
hold_course = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course);
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd);
|
||||
}
|
||||
}
|
||||
|
||||
if (hold_course != -1) {
|
||||
// recalc bearing error with hold_course;
|
||||
nav_bearing_cd = hold_course;
|
||||
// recalc bearing error
|
||||
calc_bearing_error();
|
||||
if (hold_course_cd != -1) {
|
||||
// call navigation controller for heading hold
|
||||
nav_controller->update_heading_hold(hold_course_cd);
|
||||
} else {
|
||||
nav_controller->update_level_flight();
|
||||
}
|
||||
|
||||
if (adjusted_altitude_cm() > takeoff_altitude) {
|
||||
hold_course = -1;
|
||||
hold_course_cd = -1;
|
||||
takeoff_complete = true;
|
||||
next_WP = current_loc;
|
||||
next_WP = prev_WP = current_loc;
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
@ -337,18 +337,18 @@ static bool verify_land()
|
||||
|
||||
land_complete = true;
|
||||
|
||||
if (hold_course == -1) {
|
||||
if (hold_course_cd == -1) {
|
||||
// we have just reached the threshold of to flare for landing.
|
||||
// We now don't want to do any radical
|
||||
// turns, as rolling could put the wings into the runway.
|
||||
// To prevent further turns we set hold_course to the
|
||||
// To prevent further turns we set hold_course_cd to the
|
||||
// current heading. Previously we set this to
|
||||
// crosstrack_bearing, but the xtrack bearing can easily
|
||||
// be quite large at this point, and that could induce a
|
||||
// sudden large roll correction which is very nasty at
|
||||
// this point in the landing.
|
||||
hold_course = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course);
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course_cd);
|
||||
}
|
||||
|
||||
if (g_gps->ground_speed*0.01 < 3.0) {
|
||||
@ -362,30 +362,30 @@ static bool verify_land()
|
||||
}
|
||||
}
|
||||
|
||||
if (hold_course != -1) {
|
||||
if (hold_course_cd != -1) {
|
||||
// recalc bearing error with hold_course;
|
||||
nav_bearing_cd = hold_course;
|
||||
// recalc bearing error
|
||||
calc_bearing_error();
|
||||
nav_controller->update_heading_hold(hold_course_cd);
|
||||
} else {
|
||||
update_crosstrack();
|
||||
nav_controller->update_waypoint(prev_WP, next_WP);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_nav_wp()
|
||||
{
|
||||
hold_course = -1;
|
||||
update_crosstrack();
|
||||
if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) {
|
||||
hold_course_cd = -1;
|
||||
|
||||
nav_controller->update_waypoint(prev_WP, next_WP);
|
||||
|
||||
if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
|
||||
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
|
||||
(unsigned)nav_command_index,
|
||||
(unsigned)get_distance(¤t_loc, &next_WP));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// have we circled around the waypoint?
|
||||
if (loiter_sum > 300) {
|
||||
if (loiter.loiter_sum_cd > 30000) {
|
||||
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
|
||||
return true;
|
||||
}
|
||||
@ -404,14 +404,12 @@ static bool verify_nav_wp()
|
||||
static bool verify_loiter_unlim()
|
||||
{
|
||||
update_loiter();
|
||||
calc_bearing_error();
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_loiter_time()
|
||||
{
|
||||
update_loiter();
|
||||
calc_bearing_error();
|
||||
if ((millis() - loiter_time_ms) > loiter_time_max_ms) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
|
||||
return true;
|
||||
@ -422,9 +420,8 @@ static bool verify_loiter_time()
|
||||
static bool verify_loiter_turns()
|
||||
{
|
||||
update_loiter();
|
||||
calc_bearing_error();
|
||||
if(loiter_sum > loiter_total) {
|
||||
loiter_total = 0;
|
||||
if (loiter.loiter_sum_cd > loiter.loiter_total_cd) {
|
||||
loiter.loiter_total_cd = 0;
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
|
||||
// clear the command queue;
|
||||
return true;
|
||||
@ -434,12 +431,14 @@ static bool verify_loiter_turns()
|
||||
|
||||
static bool verify_RTL()
|
||||
{
|
||||
if (wp_distance <= (uint32_t)max(g.waypoint_radius,0)) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
|
||||
return true;
|
||||
}else{
|
||||
update_loiter();
|
||||
if (wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
|
||||
nav_controller->reached_loiter_target()) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
@ -834,6 +834,6 @@
|
||||
#endif
|
||||
|
||||
#ifndef SERIAL_BUFSIZE
|
||||
# define SERIAL_BUFSIZE 256
|
||||
# define SERIAL_BUFSIZE 256
|
||||
#endif
|
||||
|
||||
|
||||
@ -1,17 +1,59 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
|
||||
// set the nav_controller pointer to the right controller
|
||||
static void set_nav_controller(void)
|
||||
{
|
||||
switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
|
||||
case AP_Navigation::CONTROLLER_L1:
|
||||
nav_controller = &L1_controller;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
reset the total loiter angle
|
||||
*/
|
||||
static void loiter_angle_reset(void)
|
||||
{
|
||||
loiter.loiter_sum_cd = 0;
|
||||
loiter.loiter_total_cd = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
update the total angle we have covered in a loiter. Used to support
|
||||
commands to do N circles of loiter
|
||||
*/
|
||||
static void loiter_angle_update(void)
|
||||
{
|
||||
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
|
||||
int32_t loiter_delta_cd;
|
||||
if (loiter.loiter_sum_cd == 0) {
|
||||
loiter_delta_cd = 0;
|
||||
} else {
|
||||
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
|
||||
}
|
||||
loiter.old_target_bearing_cd = target_bearing_cd;
|
||||
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
|
||||
|
||||
loiter.loiter_sum_cd += loiter_delta_cd;
|
||||
}
|
||||
|
||||
//****************************************************************
|
||||
// Function that will calculate the desired direction to fly and distance
|
||||
//****************************************************************
|
||||
static void navigate()
|
||||
{
|
||||
// allow change of nav controller mid-flight
|
||||
set_nav_controller();
|
||||
|
||||
// do not navigate with corrupt data
|
||||
// ---------------------------------
|
||||
if (!have_position) {
|
||||
return;
|
||||
}
|
||||
|
||||
if(next_WP.lat == 0) {
|
||||
if (next_WP.lat == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -24,24 +66,8 @@ static void navigate()
|
||||
return;
|
||||
}
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
|
||||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
|
||||
// check if we have missed the WP
|
||||
loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100;
|
||||
|
||||
// reset the old value
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// wrap values
|
||||
if (loiter_delta > 180) loiter_delta -= 360;
|
||||
if (loiter_delta < -180) loiter_delta += 360;
|
||||
loiter_sum += abs(loiter_delta);
|
||||
// update total loiter angle
|
||||
loiter_angle_update();
|
||||
|
||||
// control mode specific updates to nav_bearing
|
||||
// --------------------------------------------
|
||||
@ -49,16 +75,6 @@ static void navigate()
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
// Disabled for now
|
||||
void calc_distance_error()
|
||||
{
|
||||
distance_estimate += (float)g_gps->ground_speed * .0002f * cosf(radians(bearing_error_cd * .01f));
|
||||
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
|
||||
wp_distance = max(distance_estimate,10);
|
||||
}
|
||||
#endif
|
||||
|
||||
static void calc_airspeed_errors()
|
||||
{
|
||||
float aspeed_cm = airspeed.get_airspeed_cm();
|
||||
@ -105,12 +121,6 @@ static void calc_gndspeed_undershoot()
|
||||
}
|
||||
}
|
||||
|
||||
static void calc_bearing_error()
|
||||
{
|
||||
bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
|
||||
bearing_error_cd = wrap_180_cd(bearing_error_cd);
|
||||
}
|
||||
|
||||
static void calc_altitude_error()
|
||||
{
|
||||
if (control_mode == AUTO && offset_altitude_cm != 0) {
|
||||
@ -132,63 +142,6 @@ static void calc_altitude_error()
|
||||
|
||||
static void update_loiter()
|
||||
{
|
||||
float power;
|
||||
|
||||
if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) {
|
||||
power = float(wp_distance) / float(g.loiter_radius);
|
||||
power = constrain(power, 0.5, 1);
|
||||
nav_bearing_cd += 9000.0 * (2.0 + power) * loiter_direction;
|
||||
} else if(wp_distance < (uint32_t)max((g.loiter_radius + LOITER_RANGE),0)) {
|
||||
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
|
||||
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
|
||||
nav_bearing_cd -= power * 9000 * loiter_direction;
|
||||
} else{
|
||||
update_crosstrack();
|
||||
loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
|
||||
|
||||
}
|
||||
/*
|
||||
* if (wp_distance < g.loiter_radius){
|
||||
* nav_bearing += 9000;
|
||||
* }else{
|
||||
* nav_bearing -= 100 * M_PI / 180 * asinf(g.loiter_radius / wp_distance);
|
||||
* }
|
||||
*
|
||||
* update_crosstrack();
|
||||
*/
|
||||
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
|
||||
}
|
||||
|
||||
static void update_crosstrack(void)
|
||||
{
|
||||
// if we are using a compass for navigation, then adjust the
|
||||
// heading to account for wind
|
||||
if (g.crosstrack_use_wind && compass.use_for_yaw()) {
|
||||
Vector3f wind = ahrs.wind_estimate();
|
||||
Vector2f wind2d = Vector2f(wind.x, wind.y);
|
||||
float speed;
|
||||
if (ahrs.airspeed_estimate(&speed)) {
|
||||
Vector2f nav_vector = Vector2f(cosf(radians(nav_bearing_cd*0.01)), sinf(radians(nav_bearing_cd*0.01))) * speed;
|
||||
Vector2f nav_adjusted = nav_vector - wind2d;
|
||||
nav_bearing_cd = degrees(atan2f(nav_adjusted.y, nav_adjusted.x)) * 100;
|
||||
}
|
||||
}
|
||||
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
// If we are too far off or too close we don't do track following
|
||||
if (wp_totalDistance >= (uint32_t)max(g.crosstrack_min_distance,0) &&
|
||||
abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
|
||||
// Meters we are off track line
|
||||
crosstrack_error = sinf(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;
|
||||
nav_bearing_cd += constrain_int32(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void reset_crosstrack()
|
||||
{
|
||||
crosstrack_bearing_cd = get_bearing_cd(&prev_WP, &next_WP); // Used for track following
|
||||
nav_controller->update_loiter(next_WP, g.loiter_radius, loiter_direction);
|
||||
}
|
||||
|
||||
|
||||
@ -110,7 +110,6 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
report_radio();
|
||||
report_batt_monitor();
|
||||
report_gains();
|
||||
report_xtrack();
|
||||
report_throttle();
|
||||
report_flight_modes();
|
||||
report_ins();
|
||||
@ -544,19 +543,6 @@ static void report_gains()
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_xtrack()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Crosstrack\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
cliSerial->printf_P(PSTR("XTRACK: %4.2f\n"
|
||||
"XTRACK angle: %d\n"),
|
||||
(float)g.crosstrack_gain,
|
||||
(int)g.crosstrack_entry_angle);
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_throttle()
|
||||
{
|
||||
//print_blanks(2);
|
||||
|
||||
@ -259,6 +259,9 @@ static void init_ardupilot()
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
}
|
||||
|
||||
// choose the nav controller
|
||||
set_nav_controller();
|
||||
|
||||
set_mode(MANUAL);
|
||||
|
||||
// set the correct flight mode
|
||||
@ -352,10 +355,12 @@ static void set_mode(enum FlightMode mode)
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
prev_WP = current_loc;
|
||||
update_auto();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
prev_WP = current_loc;
|
||||
do_RTL();
|
||||
break;
|
||||
|
||||
@ -368,6 +373,7 @@ static void set_mode(enum FlightMode mode)
|
||||
break;
|
||||
|
||||
default:
|
||||
prev_WP = current_loc;
|
||||
do_RTL();
|
||||
break;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user