aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: leonard's inav2

Browse Source 
Moved loiter controller to navigation.pde
This commit is contained in:
Randy Mackay 2013-01-25 15:11:09 +09:00 committed by rmackay9
parent fd02cfe706
commit aaecc25ac7
5 changed files with 269 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/ArduCopter.pde
View File

@ -617,6 +617,9 @@ static uint16_t loiter_time_max;
static uint32_t loiter_time;
// The synthetic location created to make the copter do circles around a WP
static struct Location circle_WP;
// inertial nav loiter variables
static float loiter_lat_from_home_cm;
static float loiter_lon_from_home_cm;
////////////////////////////////////////////////////////////////////////////////
@ -1419,7 +1422,7 @@ static void update_GPS(void)
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
if (g_gps->latitude == 0) {
ground_start_count = 5;
}else{
@ -1598,7 +1601,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
roll_pitch_initialised = true;
break;
case ROLL_PITCH_LOITER_POS_VEL:
case ROLL_PITCH_LOITER:
// require gps lock
if( ap.home_is_set ) {
roll_pitch_initialised = true;
@ -1709,16 +1712,25 @@ void update_roll_pitch_mode(void)
roll_pitch_toy();
break;
case ROLL_PITCH_LOITER_POS_VEL:
case ROLL_PITCH_LOITER:
// apply SIMPLE mode transform
if(ap.simple_mode && ap_system.new_radio_frame) {
update_simple_mode();
}
// copy user input for logging purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
// Set max velocity to 2.5 m/s for conservative start
get_loiter_pos_vel(-control_pitch/(1*4.5), control_roll/(1*4.5));
// update loiter target from user controls - max velocity is 5.0 m/s
if( control_roll != 0 || control_pitch != 0 ) {
loiter_set_pos_from_velocity(-control_pitch/(2*4.5), control_roll/(2*4.5),0.01f);
}
// copy latest output from nav controller to stabilize controller
nav_roll = auto_roll;
nav_pitch = auto_pitch;
get_stabilize_roll(nav_roll);
get_stabilize_pitch(nav_pitch);
break;
}

170
ArduCopter/Attitude.pde
View File

@ -216,176 +216,6 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
}
// get_loiter_accel - loiter acceration controllers with desired accelerations provided in forward/right directions in cm/s/s
static void
get_loiter_accel(int16_t accel_req_forward, int16_t accel_req_right)
{
static float z_accel_meas = 0; // The acceleration error in cm.
static float accel_forward = 0; // The acceleration error in cm.
static float accel_right = 0; // The acceleration error in cm.
int16_t desired_roll;
int16_t desired_pitch;
z_accel_meas = -AP_INTERTIALNAV_GRAVITY * 100;
accel_forward = accel_forward + 0.11164f * (accel_req_forward - accel_forward);
accel_right = accel_right + 0.11164f * (accel_req_right - accel_right);
desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
desired_pitch = constrain((-accel_forward/(-z_accel_meas*cos_roll_x))*(18000/M_PI), -4500, 4500);
get_stabilize_roll(desired_roll);
get_stabilize_pitch(desired_pitch);
// update nav_roll and nav_pitch for reporting purposes
nav_roll = desired_roll;
nav_pitch = desired_pitch;
}
// get_loiter_accel_lat_lon - loiter acceration controller with desired accelerations provided in lat/lon directions in cm/s/s
static void
get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
{
float accel_forward;
float accel_right;
accel_forward = accel_lat*cos_yaw + accel_lon*sin_yaw;
accel_right = -accel_lat*sin_yaw + accel_lon*cos_yaw;
get_loiter_accel(accel_forward, accel_right);
}
// get_loiter_vel_lat_lon - loiter velocity controller with desired velocity provided in lat/lon directions in cm/s
#define MAX_LOITER_VEL_ACCEL 600
static void
get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon)
{
static float speed_error_lat = 0; // The velocity in cm/s.
static float speed_error_lon = 0; // The velocity in cm/s.
float speed_lat = inertial_nav.get_latitude_velocity();
float speed_lon = inertial_nav.get_longitude_velocity();
int32_t accel_lat;
int32_t accel_lon;
int32_t accel_total;
int16_t lat_p,lat_i,lat_d;
int16_t lon_p,lon_i,lon_d;
// calculate vel error and Filter with fc = 2 Hz
speed_error_lat = speed_error_lat + 0.11164f * ((vel_lat - speed_lat) - speed_error_lat);
speed_error_lon = speed_error_lon + 0.11164f * ((vel_lon - speed_lon) - speed_error_lon);
lat_p = g.pid_loiter_rate_lat.get_p(speed_error_lat);
lat_i = g.pid_loiter_rate_lat.get_i(speed_error_lat, .01f);
lat_d = g.pid_loiter_rate_lat.get_d(speed_error_lat, .01f);
lon_p = g.pid_loiter_rate_lon.get_p(speed_error_lon);
lon_i = g.pid_loiter_rate_lon.get_i(speed_error_lon, .01f);
lon_d = g.pid_loiter_rate_lon.get_d(speed_error_lon, .01f);
accel_lat = (lat_p+lat_i+lat_d);
accel_lon = (lon_p+lon_i+lon_d);
accel_total = safe_sqrt(accel_lat*accel_lat + accel_lon*accel_lon);
if( accel_total > MAX_LOITER_VEL_ACCEL ) {
accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total;
accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total;
}
get_loiter_accel_lat_lon(accel_lat, accel_lon);
}
// get_loiter_pos_lat_lon - loiter position controller with desired position provided as distance from home in lat/lon directions in cm
#define MAX_LOITER_POS_VELOCITY 1500
#define MAX_LOITER_POS_ACCEL 500
static void
get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon)
{
static float dist_error_lat;
int32_t desired_vel_lat;
static float dist_error_lon;
int32_t desired_vel_lon;
int32_t dist_error_total;
int16_t vel_sqrt;
int32_t vel_total;
int16_t linear_distance; // the distace we swap between linear and sqrt.
// calculate distance error and Filter with fc = 2 Hz
dist_error_lat = dist_error_lat + 0.11164f * ((target_lat - inertial_nav.get_latitude_diff()) - dist_error_lat);
dist_error_lon = dist_error_lon + 0.11164f * ((target_lon - inertial_nav.get_longitude_diff()) - dist_error_lon);
linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
if( dist_error_total > 2*linear_distance ) {
vel_sqrt = constrain(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000);
desired_vel_lat = vel_sqrt * dist_error_lat/dist_error_total;
desired_vel_lon = vel_sqrt * dist_error_lon/dist_error_total;
}else{
desired_vel_lat = g.pi_loiter_lat.get_p(dist_error_lat);
desired_vel_lon = g.pi_loiter_lon.get_p(dist_error_lon);
}
vel_total = safe_sqrt(desired_vel_lat*desired_vel_lat + desired_vel_lon*desired_vel_lon);
if( vel_total > MAX_LOITER_POS_VELOCITY ) {
desired_vel_lat = MAX_LOITER_POS_VELOCITY * desired_vel_lat/vel_total;
desired_vel_lon = MAX_LOITER_POS_VELOCITY * desired_vel_lon/vel_total;
}
get_loiter_vel_lat_lon(desired_vel_lat, desired_vel_lon);
}
#define MAX_LOITER_POS_VEL_VELOCITY 1000
// get_loiter_pos_vel - loiter velocity controller with desired velocity provided in front/right directions in cm/s
static void
get_loiter_pos_vel(int16_t vel_forward, int16_t vel_right)
{
static float pos_lat;
static float pos_lon;
int32_t vel_lat;
int32_t vel_lon;
int32_t vel_total;
static uint32_t last_call_ms = 0; // the last time this controller was called
uint32_t now = millis();
vel_lat = vel_forward*cos_yaw - vel_right*sin_yaw;
vel_lon = vel_forward*sin_yaw + vel_right*cos_yaw;
// reset target pos if this controller has just been engaged
if( now - last_call_ms > 100 ) {
pos_lat = inertial_nav.get_latitude_diff();
pos_lon = inertial_nav.get_longitude_diff();
}
last_call_ms = now;
// constrain the velocity vector and scale if necessary
vel_total = safe_sqrt(vel_lat*vel_lat + vel_lon*vel_lon);
if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
vel_lat = MAX_LOITER_POS_VEL_VELOCITY * vel_lat/vel_total;
vel_lon = MAX_LOITER_POS_VEL_VELOCITY * vel_lon/vel_total;
}
// To-Do: replace 0.01f with actual dT since last call
pos_lat += vel_lat * 0.01f;
pos_lon += vel_lon * 0.01f;
// call loiter position controller
get_loiter_pos_lat_lon(pos_lat, pos_lon);
}
// set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame
void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame;

3
ArduCopter/defines.h
View File

@ -30,7 +30,7 @@
#define ROLL_PITCH_AUTO 2
#define ROLL_PITCH_STABLE_OF 3
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode
#define ROLL_PITCH_LOITER_POS_VEL 5 // pilot inputs the desired horizontal velocities
#define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities
#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors
#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation
@ -199,6 +199,7 @@
#define NAV_CIRCLE 1
#define NAV_LOITER 2
#define NAV_WP 3
#define NAV_LOITER_INAV 4
// Yaw override behaviours - used for setting yaw_override_behaviour
#define YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT 0 // auto pilot takes back yaw control at next waypoint

292
ArduCopter/navigation.pde
View File

@ -1,20 +1,47 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// update_navigation - checks for new GPS updates and invokes navigation routines
// called at 50hz
static void update_navigation()
{
static uint32_t nav_last_gps_update = 0; // the system time of the last gps update
static uint32_t nav_last_gps_time = 0; // the time according to the gps
static uint32_t nav_last_update = 0; // the system time of the last time nav was run update
bool pos_updated = false;
bool log_output = false;
#if INERTIAL_NAV_XY == ENABLED
static uint8_t nav_counter = 0; // used to slow down the navigation to 10hz
// check for inertial nav updates
if( inertial_nav.position_ok() ) {
nav_counter++;
if( nav_counter >= 5) {
nav_counter = 0;
// calculate time since nav controllers last ran
dTnav = (float)(millis() - nav_last_update)/ 1000.0f;
nav_last_update = millis();
// prevent runnup in dTnav value
dTnav = min(dTnav, 1.0f);
// signal to run nav controllers
pos_updated = true;
// signal to create log entry
log_output = true;
}
}
#else
static uint32_t nav_last_gps_time = 0; // the time according to the gps
// check for new gps data
if( g_gps->fix && g_gps->time != nav_last_gps_time ) {
// used to calculate speed in X and Y, iterms
// ------------------------------------------
dTnav = (float)(millis() - nav_last_gps_update)/ 1000.0f;
nav_last_gps_update = millis();
dTnav = (float)(millis() - nav_last_update)/ 1000.0f;
nav_last_update = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0f);
@ -28,17 +55,6 @@ static void update_navigation()
// signal to create log entry
log_output = true;
}
#if INERTIAL_NAV_XY == ENABLED
// TO-DO: clean this up because inertial nav is overwriting the dTnav and pos_updated from above
// check for inertial nav updates
if( inertial_nav.position_ok() ) {
// 50hz
dTnav = 0.02f; // To-Do: calculate the time from the mainloop or INS readings?
// signal to run nav controllers
pos_updated = true;
}
#endif
// setup to calculate new navigation values and run controllers if
@ -56,8 +72,8 @@ static void update_navigation()
}
}
// reduce nav outputs to zero if we have not received a gps update in 2 seconds
if( millis() - nav_last_gps_update > 2000 ) {
// reduce nav outputs to zero if we have not seen a position update in 2 seconds
if( millis() - nav_last_update > 2000 ) {
// after 12 reads we guess we may have lost GPS signal, stop navigating
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
auto_roll >>= 1;
@ -99,6 +115,17 @@ static void run_nav_updates(void)
// from the unaltered gps locations. We do not want noise from our lead filter affecting velocity
//*******************************************************************************************************
static void calc_velocity_and_position(){
#if INERTIAL_NAV_XY == ENABLED
if( inertial_nav.position_ok() ) {
// pull velocity from interial nav library
lon_speed = inertial_nav.get_longitude_velocity();
lat_speed = inertial_nav.get_latitude_velocity();
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}
#else
static int32_t last_gps_longitude = 0;
static int32_t last_gps_latitude = 0;
@ -111,25 +138,6 @@ static void calc_velocity_and_position(){
// this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0f/dTnav;
#if INERTIAL_NAV_XY == ENABLED
if( inertial_nav.position_ok() ) {
// pull velocity from interial nav library
lon_speed = inertial_nav.get_longitude_velocity();
lat_speed = inertial_nav.get_latitude_velocity();
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}else{
// calculate velocity
lon_speed = (float)(g_gps->longitude - last_gps_longitude) * scaleLongDown * tmp;
lat_speed = (float)(g_gps->latitude - last_gps_latitude) * tmp;
// calculate position from gps + expected travel during gps_lag
current_loc.lng = xLeadFilter.get_position(g_gps->longitude, lon_speed, g_gps->get_lag());
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, lat_speed, g_gps->get_lag());
}
#else
// calculate velocity
lon_speed = (float)(g_gps->longitude - last_gps_longitude) * scaleLongDown * tmp;
lat_speed = (float)(g_gps->latitude - last_gps_latitude) * tmp;
@ -137,11 +145,11 @@ static void calc_velocity_and_position(){
// calculate position from gps + expected travel during gps_lag
current_loc.lng = xLeadFilter.get_position(g_gps->longitude, lon_speed, g_gps->get_lag());
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, lat_speed, g_gps->get_lag());
#endif
// store gps lat and lon values for next iteration
last_gps_longitude = g_gps->longitude;
last_gps_latitude = g_gps->latitude;
#endif
}
//****************************************************************
@ -238,6 +246,11 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_WP:
nav_initialised = true;
break;
case NAV_LOITER_INAV:
loiter_set_target(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff());
nav_initialised = true;
break;
}
// if initialisation has been successful update the yaw mode
@ -337,6 +350,10 @@ static void update_nav_mode()
// use error as the desired rate towards the target
calc_nav_rate(speed);
break;
case NAV_LOITER_INAV:
get_loiter_pos_lat_lon(loiter_lat_from_home_cm, loiter_lon_from_home_cm, 0.1f);
break;
}
/*
@ -358,9 +375,11 @@ static bool check_missed_wp()
return (labs(temp) > 9000); // we passed the waypoint by 100 degrees
}
////////////////////////////////////////////////////////////////
// Loiter controller (based on GPS position)
////////////////////////////////////////////////////////////////
#define NAV_ERR_MAX 600
#define NAV_RATE_ERR_MAX 250
static void calc_loiter(int16_t x_error, int16_t y_error)
{
int32_t p,i,d; // used to capture pid values for logging
@ -438,6 +457,9 @@ static void calc_loiter(int16_t x_error, int16_t y_error)
g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator());
}
///////////////////////////////////////////////////////////
// Waypoint controller (based on GPS position)
///////////////////////////////////////////////////////////
static void calc_nav_rate(int16_t max_speed)
{
float temp, temp_x, temp_y;
@ -490,16 +512,19 @@ static void calc_nav_rate(int16_t max_speed)
// We use the DCM's matrix to precalculate these trig values at 50hz
static void calc_nav_pitch_roll()
{
// rotate the vector
auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// To-Do: remove this hack dependent upon nav_mode
if( nav_mode != NAV_LOITER_INAV ) {
// rotate the vector
auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
auto_pitch = -auto_pitch;
// flip pitch because forward is negative
auto_pitch = -auto_pitch;
// constrain maximum roll and pitch angles to 45 degrees
auto_roll = constrain(auto_roll, -4500, 4500);
auto_pitch = constrain(auto_pitch, -4500, 4500);
// constrain maximum roll and pitch angles to 45 degrees
auto_roll = constrain(auto_roll, -4500, 4500);
auto_pitch = constrain(auto_pitch, -4500, 4500);
}
}
static int16_t get_desired_speed(int16_t max_speed)
@ -642,3 +667,178 @@ static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t de
{
return wrap_360(current_yaw + constrain(wrap_180(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
}
////////////////////////////////////////////////////
// Loiter controller using inertial nav
////////////////////////////////////////////////////
// get_loiter_accel - loiter acceration controllers with desired accelerations provided in forward/right directions in cm/s/s
static void
get_loiter_accel(int16_t accel_req_forward, int16_t accel_req_right)
{
static float z_accel_meas = 0; // The acceleration error in cm.
static float accel_forward = 0; // The acceleration error in cm.
static float accel_right = 0; // The acceleration error in cm.
z_accel_meas = -AP_INTERTIALNAV_GRAVITY * 100;
// calculate accel and filter with fc = 2 Hz
// 100hz sample rate, 2hz filter, alpha = 0.11164f
// 20hz sample rate, 2hz filter, alpha = 0.38587f
// 10hz sample rate, 2hz filter, alpha = 0.55686f
accel_forward = accel_forward + 0.55686f * (accel_req_forward - accel_forward);
accel_right = accel_right + 0.55686f * (accel_req_right - accel_right);
// update angle targets that will be passed to stabilize controller
auto_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
auto_pitch = constrain((-accel_forward/(-z_accel_meas*cos_roll_x))*(18000/M_PI), -4500, 4500);
}
// get_loiter_accel_lat_lon - loiter acceration controller with desired accelerations provided in lat/lon directions in cm/s/s
static void
get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
{
float accel_forward;
float accel_right;
accel_forward = accel_lat*cos_yaw + accel_lon*sin_yaw;
accel_right = -accel_lat*sin_yaw + accel_lon*cos_yaw;
get_loiter_accel(accel_forward, accel_right);
}
// get_loiter_vel_lat_lon - loiter velocity controller with desired velocity provided in lat/lon directions in cm/s
#define MAX_LOITER_VEL_ACCEL 400 // should be 1.5 times larger than MAX_LOITER_POS_ACCEL
static void
get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt)
{
static float speed_error_lat = 0; // The velocity in cm/s.
static float speed_error_lon = 0; // The velocity in cm/s.
float speed_lat = inertial_nav.get_latitude_velocity();
float speed_lon = inertial_nav.get_longitude_velocity();
int32_t accel_lat;
int32_t accel_lon;
int32_t accel_total;
int16_t lat_p,lat_i,lat_d;
int16_t lon_p,lon_i,lon_d;
// calculate vel error and Filter with fc = 2 Hz
// 100hz sample rate, 2hz filter, alpha = 0.11164f
// 20hz sample rate, 2hz filter, alpha = 0.38587f
// 10hz sample rate, 2hz filter, alpha = 0.55686f
speed_error_lat = speed_error_lat + 0.55686f * ((vel_lat - speed_lat) - speed_error_lat);
speed_error_lon = speed_error_lon + 0.55686f * ((vel_lon - speed_lon) - speed_error_lon);
lat_p = g.pid_loiter_rate_lat.get_p(speed_error_lat);
lat_i = g.pid_loiter_rate_lat.get_i(speed_error_lat, dt);
lat_d = g.pid_loiter_rate_lat.get_d(speed_error_lat, dt);
lon_p = g.pid_loiter_rate_lon.get_p(speed_error_lon);
lon_i = g.pid_loiter_rate_lon.get_i(speed_error_lon, dt);
lon_d = g.pid_loiter_rate_lon.get_d(speed_error_lon, dt);
accel_lat = (lat_p+lat_i+lat_d);
accel_lon = (lon_p+lon_i+lon_d);
accel_total = safe_sqrt(accel_lat*accel_lat + accel_lon*accel_lon);
if( accel_total > MAX_LOITER_VEL_ACCEL ) {
accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total;
accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total;
}
get_loiter_accel_lat_lon(accel_lat, accel_lon);
}
// get_loiter_pos_lat_lon - loiter position controller with desired position provided as distance from home in lat/lon directions in cm
#define MAX_LOITER_POS_VELOCITY 750 // should be 1.5 ~ 2.0 times the pilot input's max velocity
#define MAX_LOITER_POS_ACCEL 250
static void
get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon, float dt)
{
static float dist_error_lat;
int32_t desired_vel_lat;
static float dist_error_lon;
int32_t desired_vel_lon;
int32_t dist_error_total;
int16_t vel_sqrt;
int32_t vel_total;
int16_t linear_distance; // the distace we swap between linear and sqrt.
// calculate distance error and Filter with fc = 2 Hz
// 100hz sample rate, 2hz filter, alpha = 0.11164f
// 20hz sample rate, 2hz filter, alpha = 0.38587f
// 10hz sample rate, 2hz filter, alpha = 0.55686f
dist_error_lat = dist_error_lat + 0.55686f * ((target_lat - inertial_nav.get_latitude_diff()) - dist_error_lat);
dist_error_lon = dist_error_lon + 0.55686f * ((target_lon - inertial_nav.get_longitude_diff()) - dist_error_lon);
linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP());
dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
if( dist_error_total > 2*linear_distance ) {
vel_sqrt = constrain(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000);
desired_vel_lat = vel_sqrt * dist_error_lat/dist_error_total;
desired_vel_lon = vel_sqrt * dist_error_lon/dist_error_total;
}else{
desired_vel_lat = g.pi_loiter_lat.get_p(dist_error_lat);
desired_vel_lon = g.pi_loiter_lon.get_p(dist_error_lon);
}
vel_total = safe_sqrt(desired_vel_lat*desired_vel_lat + desired_vel_lon*desired_vel_lon);
if( vel_total > MAX_LOITER_POS_VELOCITY ) {
desired_vel_lat = MAX_LOITER_POS_VELOCITY * desired_vel_lat/vel_total;
desired_vel_lon = MAX_LOITER_POS_VELOCITY * desired_vel_lon/vel_total;
}
get_loiter_vel_lat_lon(desired_vel_lat, desired_vel_lon, dt);
}
#define MAX_LOITER_POS_VEL_VELOCITY 1000
// loiter_set_pos_from_velocity - loiter velocity controller with desired velocity provided in front/right directions in cm/s
static void
loiter_set_pos_from_velocity(int16_t vel_forward_cms, int16_t vel_right_cms, float dt)
{
int32_t vel_lat;
int32_t vel_lon;
int32_t vel_total;
vel_lat = vel_forward_cms*cos_yaw - vel_right_cms*sin_yaw;
vel_lon = vel_forward_cms*sin_yaw + vel_right_cms*cos_yaw;
// constrain the velocity vector and scale if necessary
vel_total = safe_sqrt(vel_lat*vel_lat + vel_lon*vel_lon);
if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
vel_lat = MAX_LOITER_POS_VEL_VELOCITY * vel_lat/vel_total;
vel_lon = MAX_LOITER_POS_VEL_VELOCITY * vel_lon/vel_total;
}
// update loiter target position
loiter_lat_from_home_cm += vel_lat * dt;
loiter_lon_from_home_cm += vel_lon * dt;
// update next_WP location for reporting purposes
next_WP.lat = home.lat + loiter_lat_from_home_cm;
next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp;
}
// loiter_set_target - set loiter's target position from home in cm
static void
loiter_set_target(float lat_from_home_cm, float lon_from_home_cm)
{
loiter_lat_from_home_cm = lat_from_home_cm;
loiter_lon_from_home_cm = lon_from_home_cm;
// update next_WP location for reporting purposes
next_WP.lat = home.lat + loiter_lat_from_home_cm;
next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp;
}

9
ArduCopter/system.pde
View File

@ -447,16 +447,12 @@ static void set_mode(uint8_t mode)
ap.manual_throttle = false;
ap.manual_attitude = false;
// start circling around current location
set_next_WP(&current_loc);
circle_WP = next_WP;
// set yaw to point to center of circle
yaw_look_at_WP = circle_WP;
set_yaw_mode(YAW_LOOK_AT_LOCATION);
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
circle_angle = 0;
set_nav_mode(NAV_CIRCLE);
break;
case LOITER:
@ -466,6 +462,7 @@ static void set_mode(uint8_t mode)
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(LOITER_THR);
set_next_WP(&current_loc);
set_nav_mode(NAV_LOITER);
break;
case POSITION:
@ -475,6 +472,7 @@ static void set_mode(uint8_t mode)
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(THROTTLE_MANUAL);
set_next_WP(&current_loc);
set_nav_mode(NAV_LOITER);
break;
case GUIDED:
@ -516,6 +514,7 @@ static void set_mode(uint8_t mode)
set_yaw_mode(OF_LOITER_YAW);
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_nav_mode(NAV_NONE);
set_next_WP(&current_loc);
break;