diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 46df00d2d2..e1baed8074 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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; } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 4351ef5f14..cc796ff706 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 76ade76a45..fa7d14914b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index d202edfbd6..8d0f111ac5 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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; +} diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6076225877..59896b2d6e 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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(¤t_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(¤t_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(¤t_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(¤t_loc); break;