mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
2.2B6 - Please verify Heli still functions properly.
Added AC_PID lib Updated landing code bug fixes
This commit is contained in:
Jason Short
parent
5185901f79
commit
0213f4dd88
@ -1,6 +1,6 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.2 b4"
|
||||
#define THISFIRMWARE "ArduCopter V2.2 b6"
|
||||
/*
|
||||
ArduCopter Version 2.2
|
||||
Authors: Jason Short
|
||||
@ -69,7 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
|
||||
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
|
||||
#include <AP_DCM.h> // ArduPilot Mega DCM Library
|
||||
#include <APM_PI.h> // PI library
|
||||
#include <PID.h> // PID library
|
||||
#include <AC_PID.h> // PID library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <AP_OpticalFlow.h> // Optical Flow library
|
||||
@ -312,11 +312,6 @@ static const char* flight_mode_strings[] = {
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The GPS based velocity calculated by offsetting the Latitude and Longitude
|
||||
// updated after GPS read - 5-10hz
|
||||
static int16_t x_GPS_speed;
|
||||
static int16_t y_GPS_speed;
|
||||
|
||||
// The synthesized velocity calculated by fancy filtering and fusion
|
||||
// updated at 50hz
|
||||
static int16_t x_actual_speed;
|
||||
static int16_t y_actual_speed;
|
||||
|
||||
@ -466,6 +461,8 @@ static uint8_t wp_verify_byte; // used for tracking state of navigating wa
|
||||
static int16_t waypoint_speed_gov;
|
||||
// Used to track how many cm we are from the "next_WP" location
|
||||
static int32_t long_error, lat_error;
|
||||
// Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s
|
||||
static boolean loiter_override;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -554,19 +551,16 @@ static int32_t altitude_error;
|
||||
static int16_t climb_rate;
|
||||
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
|
||||
static int16_t sonar_alt;
|
||||
// The previous altitude as reported by Sonar in cm for calculation of Climb Rate
|
||||
static int16_t old_sonar_alt;
|
||||
// The climb_rate as reported by sonar in cm/s
|
||||
static int16_t sonar_rate;
|
||||
// The altitude as reported by Baro in cm – Values can be quite high
|
||||
static int32_t baro_alt;
|
||||
// The previous altitude as reported by Baro in cm for calculation of Climb Rate
|
||||
static int32_t old_baro_alt;
|
||||
// The climb_rate as reported by Baro in cm/s
|
||||
static int16_t baro_rate;
|
||||
//
|
||||
static boolean reset_throttle_flag;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// flight modes
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -598,6 +592,10 @@ static int16_t manual_boost;
|
||||
static int16_t angle_boost;
|
||||
// Push copter down for clean landing
|
||||
static int16_t landing_boost;
|
||||
// for controlling the landing throttle curve
|
||||
//verifies landings
|
||||
static int16_t ground_detector;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation general
|
||||
@ -823,7 +821,7 @@ void loop()
|
||||
uint32_t timer = micros();
|
||||
// We want this to execute fast
|
||||
// ----------------------------
|
||||
if ((timer - fast_loopTimer) >= 5000) {
|
||||
if ((timer - fast_loopTimer) >= 4500) {
|
||||
//PORTK |= B00010000;
|
||||
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
|
||||
fast_loopTimer = timer;
|
||||
@ -849,7 +847,7 @@ void loop()
|
||||
|
||||
// update our velocity estimate based on IMU at 50hz
|
||||
// -------------------------------------------------
|
||||
estimate_velocity();
|
||||
//estimate_velocity();
|
||||
|
||||
// check for new GPS messages
|
||||
// --------------------------
|
||||
@ -900,6 +898,12 @@ static void fast_loop()
|
||||
// IMU DCM Algorithm
|
||||
read_AHRS();
|
||||
|
||||
if(takeoff_complete == false){
|
||||
// reset these I terms to prevent awkward tipping on takeoff
|
||||
reset_rate_I();
|
||||
reset_stability_I();
|
||||
}
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
update_yaw_mode();
|
||||
@ -1515,11 +1519,6 @@ void update_throttle_mode(void)
|
||||
if ((millis() - takeoff_timer) > 5000){
|
||||
// we must be in the air by now
|
||||
takeoff_complete = true;
|
||||
land_complete = false;
|
||||
}else{
|
||||
// reset these I terms to prevent awkward tipping on takeoff
|
||||
reset_rate_I();
|
||||
reset_stability_I();
|
||||
}
|
||||
}else{
|
||||
// we are on the ground
|
||||
@ -1532,11 +1531,6 @@ void update_throttle_mode(void)
|
||||
// reset_baro();
|
||||
}
|
||||
|
||||
// reset out i terms and takeoff timer
|
||||
// -----------------------------------
|
||||
reset_rate_I();
|
||||
reset_stability_I();
|
||||
|
||||
// remember our time since takeoff
|
||||
// -------------------------------
|
||||
takeoff_timer = millis();
|
||||
@ -1593,12 +1587,8 @@ void update_throttle_mode(void)
|
||||
// how far off are we
|
||||
altitude_error = get_altitude_error();
|
||||
|
||||
// get the AP throttle, if landing boost > 0 we are actually Landing
|
||||
// This allows us to grab just the compensation.
|
||||
if(landing_boost > 0)
|
||||
nav_throttle = get_nav_throttle(-200);
|
||||
else
|
||||
nav_throttle = get_nav_throttle(altitude_error);
|
||||
// get the AP throttle
|
||||
nav_throttle = get_nav_throttle(altitude_error);
|
||||
|
||||
// clear the new data flag
|
||||
invalid_throttle = false;
|
||||
@ -1612,6 +1602,11 @@ void update_throttle_mode(void)
|
||||
//*/
|
||||
}
|
||||
|
||||
// hack to remove the influence of the ground effect
|
||||
if(current_loc.alt < 200 && landing_boost != 0) {
|
||||
nav_throttle = min(nav_throttle, 0);
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost);
|
||||
#else
|
||||
@ -1659,24 +1654,18 @@ static void update_navigation()
|
||||
case RTL:
|
||||
// We have reached Home
|
||||
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
|
||||
// if this value > 0, we are set to trigger auto_land after 30 seconds
|
||||
// if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds
|
||||
set_mode(LOITER);
|
||||
auto_land_timer = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
// We wait until we've reached out new altitude before coming home
|
||||
// Arg doesn't work, it
|
||||
//if(alt_change_flag != REACHED_ALT){
|
||||
// wp_control = NO_NAV_MODE;
|
||||
//}else{
|
||||
wp_control = WP_MODE;
|
||||
wp_control = WP_MODE;
|
||||
|
||||
// calculates desired Yaw
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_auto_yaw();
|
||||
#endif
|
||||
//}
|
||||
// calculates desired Yaw
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_auto_yaw();
|
||||
#endif
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
@ -1687,7 +1676,15 @@ static void update_navigation()
|
||||
case POSITION:
|
||||
// This feature allows us to reposition the quad when the user lets
|
||||
// go of the sticks
|
||||
|
||||
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){
|
||||
// flag will reset when speed drops below .5 m/s
|
||||
loiter_override = true;
|
||||
}
|
||||
|
||||
// Allow the user to take control temporarily,
|
||||
// regain hold when the speed goes down to .5m/s
|
||||
if(loiter_override){
|
||||
// this sets the copter to not try and nav while we control it
|
||||
wp_control = NO_NAV_MODE;
|
||||
|
||||
@ -1695,8 +1692,11 @@ static void update_navigation()
|
||||
next_WP.lat = current_loc.lat;
|
||||
next_WP.lng = current_loc.lng;
|
||||
|
||||
if(g_gps->ground_speed < 50){
|
||||
loiter_override = false;
|
||||
wp_control = LOITER_MODE;
|
||||
}
|
||||
}else{
|
||||
// this is also set by GPS in update_nav
|
||||
wp_control = LOITER_MODE;
|
||||
}
|
||||
|
||||
@ -1712,7 +1712,10 @@ static void update_navigation()
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
verify_land();
|
||||
if(g.sonar_enabled)
|
||||
verify_land_sonar();
|
||||
else
|
||||
verify_land_baro();
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
@ -1795,6 +1798,10 @@ static void update_trig(void){
|
||||
// updated at 10hz
|
||||
static void update_altitude()
|
||||
{
|
||||
static int16_t old_sonar_alt = 0;
|
||||
static int32_t old_baro_alt = 0;
|
||||
static int16_t old_climb_rate = 0;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||
// we are in the SIM, fake out the baro and Sonar
|
||||
int fake_relative_alt = g_gps->altitude - gps_base_alt;
|
||||
@ -1815,7 +1822,7 @@ static void update_altitude()
|
||||
baro_rate = (temp + baro_rate) >> 1;
|
||||
old_baro_alt = baro_alt;
|
||||
|
||||
// sonar_alt is calculated in a faster loop and filtered with a mode filter
|
||||
// Note: sonar_alt is calculated in a faster loop and filtered with a mode filter
|
||||
#endif
|
||||
|
||||
|
||||
@ -1865,9 +1872,15 @@ static void update_altitude()
|
||||
climb_rate = baro_rate;
|
||||
}
|
||||
|
||||
// simple smoothing
|
||||
climb_rate = (climb_rate + old_climb_rate)>>1;
|
||||
|
||||
// manage bad data
|
||||
climb_rate = constrain(climb_rate, -300, 300);
|
||||
|
||||
// save for filtering
|
||||
old_climb_rate = climb_rate;
|
||||
|
||||
// update the target altitude
|
||||
next_WP.alt = get_new_altitude();
|
||||
}
|
||||
@ -1896,14 +1909,18 @@ static void tuning(){
|
||||
switch(g.radio_tuning){
|
||||
|
||||
case CH6_DAMP:
|
||||
g.rc_6.set_range(0,80); // 0 to 1
|
||||
g.rc_6.set_range(0,300); // 0 to 1
|
||||
g.stablize_d.set(tuning_value);
|
||||
|
||||
//g.rc_6.set_range(0,60); // 0 to 1
|
||||
//g.pid_rate_roll.kD(tuning_value);
|
||||
//g.pid_rate_pitch.kD(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_STABILIZE_KP:
|
||||
g.rc_6.set_range(0,8000); // 0 to 8
|
||||
g.pi_stabilize_roll.kP(tuning_value);
|
||||
g.pi_stabilize_pitch.kP(tuning_value);
|
||||
g.pid_rate_roll.kP(tuning_value);
|
||||
g.pid_rate_pitch.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_STABILIZE_KI:
|
||||
@ -1915,16 +1932,14 @@ static void tuning(){
|
||||
|
||||
case CH6_RATE_KP:
|
||||
g.rc_6.set_range(40,300); // 0 to .3
|
||||
g.pi_rate_roll.kP(tuning_value);
|
||||
g.pi_rate_pitch.kP(tuning_value);
|
||||
g.pi_acro_roll.kP(tuning_value);
|
||||
g.pi_acro_pitch.kP(tuning_value);
|
||||
g.pid_rate_roll.kP(tuning_value);
|
||||
g.pid_rate_pitch.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_RATE_KI:
|
||||
g.rc_6.set_range(0,300); // 0 to .3
|
||||
g.pi_rate_roll.kI(tuning_value);
|
||||
g.pi_rate_pitch.kI(tuning_value);
|
||||
g.pid_rate_roll.kI(tuning_value);
|
||||
g.pid_rate_pitch.kI(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_YAW_KP:
|
||||
@ -1934,12 +1949,12 @@ static void tuning(){
|
||||
|
||||
case CH6_YAW_RATE_KP:
|
||||
g.rc_6.set_range(0,1000);
|
||||
g.pi_rate_yaw.kP(tuning_value);
|
||||
g.pid_rate_yaw.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_THROTTLE_KP:
|
||||
g.rc_6.set_range(0,1000); // 0 to 1
|
||||
g.pi_throttle.kP(tuning_value);
|
||||
g.pid_throttle.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_TOP_BOTTOM_RATIO:
|
||||
@ -1966,8 +1981,8 @@ static void tuning(){
|
||||
|
||||
case CH6_NAV_P:
|
||||
g.rc_6.set_range(0,6000);
|
||||
g.pi_nav_lat.kP(tuning_value);
|
||||
g.pi_nav_lon.kP(tuning_value);
|
||||
g.pid_nav_lat.kP(tuning_value);
|
||||
g.pid_nav_lon.kP(tuning_value);
|
||||
break;
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
@ -1984,20 +1999,20 @@ static void tuning(){
|
||||
|
||||
case CH6_OPTFLOW_KP:
|
||||
g.rc_6.set_range(0,5000); // 0 to 5
|
||||
g.pi_optflow_roll.kP(tuning_value);
|
||||
g.pi_optflow_pitch.kP(tuning_value);
|
||||
g.pid_optflow_roll.kP(tuning_value);
|
||||
g.pid_optflow_pitch.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_OPTFLOW_KI:
|
||||
g.rc_6.set_range(0,10000); // 0 to 10
|
||||
g.pi_optflow_roll.kI(tuning_value);
|
||||
g.pi_optflow_pitch.kI(tuning_value);
|
||||
g.pid_optflow_roll.kI(tuning_value);
|
||||
g.pid_optflow_pitch.kI(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_OPTFLOW_KD:
|
||||
g.rc_6.set_range(0,200); // 0 to 0.2
|
||||
g.pi_optflow_roll.kD(tuning_value);
|
||||
g.pi_optflow_pitch.kD(tuning_value);
|
||||
g.pid_optflow_roll.kD(tuning_value);
|
||||
g.pid_optflow_pitch.kD(tuning_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -2075,8 +2090,9 @@ static void update_nav_wp()
|
||||
|
||||
}else if(wp_control == NO_NAV_MODE){
|
||||
// clear out our nav so we can do things like land straight down
|
||||
// or change Loiter position
|
||||
|
||||
// We bring in our iterms for wind control, but we don't navigate
|
||||
// We bring copy over our Iterms for wind control, but we don't navigate
|
||||
nav_lon = g.pi_loiter_lon.get_integrator();
|
||||
nav_lat = g.pi_loiter_lat.get_integrator();
|
||||
|
||||
|
||||
@ -3,134 +3,131 @@
|
||||
static int
|
||||
get_stabilize_roll(int32_t target_angle)
|
||||
{
|
||||
int32_t error = 0;
|
||||
int32_t rate = 0;
|
||||
|
||||
static float current_rate = 0;
|
||||
|
||||
// angle error
|
||||
error = wrap_180(target_angle - dcm.roll_sensor);
|
||||
target_angle = wrap_180(target_angle - dcm.roll_sensor);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -4500, 4500);
|
||||
target_angle = constrain(target_angle, -4500, 4500);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
|
||||
target_angle = g.pi_stabilize_roll.get_pi(target_angle, G_Dt);
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
return (int)rate;
|
||||
return constrain(target_angle, -4500, 4500);
|
||||
#else
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -2500, 2500);
|
||||
target_angle = constrain(target_angle, -2500, 2500);
|
||||
|
||||
// conver to desired Rate:
|
||||
rate = g.pi_stabilize_roll.get_p(error);
|
||||
int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
|
||||
int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt);
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
||||
|
||||
// rate control
|
||||
error = rate - (omega.x * DEGX100);
|
||||
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||
|
||||
// D term
|
||||
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
|
||||
int16_t d_temp = current_rate * g.stablize_d;
|
||||
rate -= d_temp;
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -2500, 2500);
|
||||
return (int)rate + iterm;
|
||||
return get_rate_roll(target_rate) + iterm;
|
||||
#endif
|
||||
}
|
||||
|
||||
static int
|
||||
get_stabilize_pitch(int32_t target_angle)
|
||||
{
|
||||
int32_t error = 0;
|
||||
int32_t rate = 0;
|
||||
static float current_rate = 0;
|
||||
|
||||
// angle error
|
||||
error = wrap_180(target_angle - dcm.pitch_sensor);
|
||||
target_angle = wrap_180(target_angle - dcm.pitch_sensor);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -4500, 4500);
|
||||
target_angle = constrain(target_angle, -4500, 4500);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
|
||||
target_angle = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt);
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
return (int)rate;
|
||||
return constrain(target_angle, -4500, 4500);
|
||||
#else
|
||||
// angle error
|
||||
error = constrain(error, -2500, 2500);
|
||||
// limit the error we're feeding to the PID
|
||||
target_angle = constrain(target_angle, -2500, 2500);
|
||||
|
||||
// conver to desired Rate:
|
||||
rate = g.pi_stabilize_pitch.get_p(error);
|
||||
int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
|
||||
int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt);
|
||||
|
||||
// rate control
|
||||
error = rate - (omega.y * DEGX100);
|
||||
|
||||
//error = rate - (omega.y * DEGX100);
|
||||
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||
|
||||
// D term
|
||||
current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3;
|
||||
int16_t d_temp = current_rate * g.stablize_d;
|
||||
rate -= d_temp;
|
||||
|
||||
// output control:
|
||||
rate = constrain(rate, -2500, 2500);
|
||||
return (int)rate + iterm;
|
||||
return get_rate_pitch(target_rate) + iterm;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#define YAW_ERROR_MAX 2000
|
||||
static int
|
||||
get_stabilize_yaw(int32_t target_angle)
|
||||
{
|
||||
int32_t error;
|
||||
int32_t rate;
|
||||
|
||||
// angle error
|
||||
error = wrap_180(target_angle - dcm.yaw_sensor);
|
||||
target_angle = wrap_180(target_angle - dcm.yaw_sensor);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
|
||||
target_angle = constrain(target_angle, -2000, 2000);
|
||||
|
||||
// convert to desired Rate:
|
||||
rate = g.pi_stabilize_yaw.get_p(error);
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
|
||||
// conver to desired Rate:
|
||||
int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle);
|
||||
int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
||||
if( !g.heli_ext_gyro_enabled ) {
|
||||
error = rate - (omega.z * DEGX100);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
if(!g.heli_ext_gyro_enabled){
|
||||
return get_rate_yaw(target_rate) + iterm;
|
||||
}else{
|
||||
return constrain((target_rate + iterm), -4500, 4500);
|
||||
}
|
||||
// output control:
|
||||
rate = constrain(rate, -4500, 4500);
|
||||
#else
|
||||
error = rate - (omega.z * DEGX100);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
return get_rate_yaw(target_rate) + iterm;
|
||||
#endif
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_roll(int32_t target_rate)
|
||||
{
|
||||
static int32_t last_rate = 0;
|
||||
int32_t current_rate = (omega.x * DEGX100);
|
||||
|
||||
// rate control
|
||||
target_rate = target_rate - current_rate;
|
||||
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
|
||||
|
||||
// Dampening
|
||||
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
|
||||
last_rate = current_rate;
|
||||
|
||||
// output control:
|
||||
int16_t yaw_input = 1400 + abs(g.rc_4.control_in);
|
||||
// smoother Yaw control:
|
||||
rate = constrain(rate, -yaw_input, yaw_input);
|
||||
#endif
|
||||
return constrain(target_rate, -2500, 2500);
|
||||
}
|
||||
|
||||
return (int)rate + iterm;
|
||||
static int
|
||||
get_rate_pitch(int32_t target_rate)
|
||||
{
|
||||
static int32_t last_rate = 0;
|
||||
int32_t current_rate = (omega.y * DEGX100);
|
||||
|
||||
// rate control
|
||||
target_rate = target_rate - current_rate;
|
||||
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
|
||||
|
||||
// Dampening
|
||||
target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500);
|
||||
last_rate = current_rate;
|
||||
|
||||
// output control:
|
||||
return constrain(target_rate, -2500, 2500);
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_yaw(int32_t target_rate)
|
||||
{
|
||||
// rate control
|
||||
target_rate = target_rate - (omega.z * DEGX100);
|
||||
target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt);
|
||||
|
||||
// output control:
|
||||
int16_t yaw_limit = 1400 + abs(g.rc_4.control_in);
|
||||
|
||||
// smoother Yaw control:
|
||||
return constrain(target_rate, -yaw_limit, yaw_limit);
|
||||
}
|
||||
|
||||
#define ALT_ERROR_MAX 400
|
||||
@ -138,10 +135,8 @@ static int16_t
|
||||
get_nav_throttle(int32_t z_error)
|
||||
{
|
||||
static int16_t old_output = 0;
|
||||
//static int16_t rate_d = 0;
|
||||
|
||||
int16_t rate_error;
|
||||
int16_t output;
|
||||
int16_t rate_error = 0;
|
||||
int16_t output = 0;
|
||||
|
||||
// limit error to prevent I term run up
|
||||
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
||||
@ -155,17 +150,10 @@ get_nav_throttle(int32_t z_error)
|
||||
// calculate rate error
|
||||
rate_error = rate_error - climb_rate;
|
||||
|
||||
// limit the rate - iterm is not used
|
||||
output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
|
||||
|
||||
// a positive climb rate means we're going up
|
||||
//rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain
|
||||
|
||||
// slight adjustment to alt hold output
|
||||
//output -= constrain(rate_d, -25, 25);
|
||||
// limit the rate
|
||||
output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180);
|
||||
|
||||
// light filter of output
|
||||
//output = (old_output * 3 + output) / 4;
|
||||
output = (old_output + output) / 2;
|
||||
|
||||
// save our output
|
||||
@ -175,33 +163,6 @@ get_nav_throttle(int32_t z_error)
|
||||
return output + iterm;
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_roll(int32_t target_rate)
|
||||
{
|
||||
int32_t error = (target_rate * 3.5) - (omega.x * DEGX100);
|
||||
error = constrain(error, -20000, 20000);
|
||||
return g.pi_acro_roll.get_pi(error, G_Dt);
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_pitch(int32_t target_rate)
|
||||
{
|
||||
int32_t error = (target_rate * 3.5) - (omega.y * DEGX100);
|
||||
error = constrain(error, -20000, 20000);
|
||||
return g.pi_acro_pitch.get_pi(error, G_Dt);
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_yaw(int32_t target_rate)
|
||||
{
|
||||
|
||||
int32_t error = (target_rate * 4.5) - (omega.z * DEGX100);
|
||||
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
return (int)constrain(target_rate, -2500, 2500);
|
||||
}
|
||||
|
||||
// Keeps old data out of our calculation / logs
|
||||
static void reset_nav_params(void)
|
||||
{
|
||||
@ -247,17 +208,15 @@ static void reset_I_all(void)
|
||||
|
||||
static void reset_rate_I()
|
||||
{
|
||||
g.pi_rate_roll.reset_I();
|
||||
g.pi_rate_pitch.reset_I();
|
||||
g.pi_acro_roll.reset_I();
|
||||
g.pi_acro_pitch.reset_I();
|
||||
g.pi_rate_yaw.reset_I();
|
||||
g.pid_rate_roll.reset_I();
|
||||
g.pid_rate_pitch.reset_I();
|
||||
g.pid_rate_yaw.reset_I();
|
||||
}
|
||||
|
||||
static void reset_optflow_I(void)
|
||||
{
|
||||
g.pi_optflow_roll.reset_I();
|
||||
g.pi_optflow_pitch.reset_I();
|
||||
g.pid_optflow_roll.reset_I();
|
||||
g.pid_optflow_pitch.reset_I();
|
||||
of_roll = 0;
|
||||
of_pitch = 0;
|
||||
}
|
||||
@ -272,15 +231,15 @@ static void reset_wind_I(void)
|
||||
static void reset_nav_I(void)
|
||||
{
|
||||
// Rate control for WP navigation
|
||||
g.pi_nav_lat.reset_I();
|
||||
g.pi_nav_lon.reset_I();
|
||||
g.pid_nav_lat.reset_I();
|
||||
g.pid_nav_lon.reset_I();
|
||||
}
|
||||
|
||||
static void reset_throttle_I(void)
|
||||
{
|
||||
// For Altitude Hold
|
||||
g.pi_alt_hold.reset_I();
|
||||
g.pi_throttle.reset_I();
|
||||
g.pid_throttle.reset_I();
|
||||
}
|
||||
|
||||
static void reset_stability_I(void)
|
||||
@ -325,7 +284,7 @@ static int get_angle_boost(int value)
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = 1.0 - constrain(temp, .5, 1.0);
|
||||
int16_t output = temp * value;
|
||||
return constrain(output, 0, 100);
|
||||
return constrain(output, 0, 200);
|
||||
// return (int)(temp * value);
|
||||
}
|
||||
|
||||
@ -482,9 +441,9 @@ get_of_roll(int32_t control_roll)
|
||||
|
||||
// only stop roll if caller isn't modifying roll
|
||||
if( control_roll == 0 && current_loc.alt < 1500) {
|
||||
new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
|
||||
//new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
|
||||
}else{
|
||||
g.pi_optflow_roll.reset_I();
|
||||
g.pid_optflow_roll.reset_I();
|
||||
tot_x_cm = 0;
|
||||
}
|
||||
// limit amount of change and maximum angle
|
||||
@ -516,10 +475,10 @@ get_of_pitch(int32_t control_pitch)
|
||||
|
||||
// only stop roll if caller isn't modifying pitch
|
||||
if( control_pitch == 0 && current_loc.alt < 1500 ) {
|
||||
new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
|
||||
//new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change
|
||||
}else{
|
||||
tot_y_cm = 0;
|
||||
g.pi_optflow_pitch.reset_I();
|
||||
g.pid_optflow_pitch.reset_I();
|
||||
}
|
||||
|
||||
// limit amount of change
|
||||
|
||||
@ -824,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
break;
|
||||
|
||||
case MAV_ACTION_EMCY_LAND:
|
||||
//set_mode(LAND);
|
||||
set_mode(LAND);
|
||||
break;
|
||||
|
||||
case MAV_ACTION_HALT:
|
||||
@ -1529,6 +1529,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
imu.set_gyro(gyros);
|
||||
*/
|
||||
//imu.set_accel(accels);
|
||||
|
||||
// rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM
|
||||
Vector3f gyros;
|
||||
gyros.x = (float)packet.rollspeed / 1000.0;
|
||||
gyros.y = (float)packet.pitchspeed / 1000.0;
|
||||
gyros.z = (float)packet.yawspeed / 1000.0;
|
||||
|
||||
imu.set_gyro(gyros);
|
||||
|
||||
// m/s/s
|
||||
Vector3f accels;
|
||||
accels.x = (float)packet.roll * gravity / 1000.0;
|
||||
accels.y = (float)packet.pitch * gravity / 1000.0;
|
||||
accels.z = (float)packet.yaw * gravity / 1000.0;
|
||||
|
||||
imu.set_accel(accels);
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -590,7 +590,7 @@ static void Log_Write_Control_Tuning()
|
||||
DataFlash.WriteInt(climb_rate); // 10
|
||||
DataFlash.WriteInt(g.rc_3.servo_out); // 11
|
||||
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12
|
||||
DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13
|
||||
DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
@ -17,7 +17,7 @@ public:
|
||||
// The increment will prevent old parameters from being used incorrectly
|
||||
// by newer code.
|
||||
//
|
||||
static const uint16_t k_format_version = 114;
|
||||
static const uint16_t k_format_version = 115;
|
||||
|
||||
// The parameter software_type is set up solely for ground station use
|
||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||
@ -26,7 +26,6 @@ public:
|
||||
//
|
||||
static const uint16_t k_software_type = 10; // 0 for APM trunk
|
||||
|
||||
//
|
||||
// Parameter identities.
|
||||
//
|
||||
// The enumeration defined here is used to ensure that every parameter
|
||||
@ -168,23 +167,20 @@ public:
|
||||
// 235: PI/D Controllers
|
||||
//
|
||||
k_param_stabilize_d = 234,
|
||||
k_param_pi_rate_roll = 235,
|
||||
k_param_pi_rate_pitch,
|
||||
k_param_pi_rate_yaw,
|
||||
k_param_pid_rate_roll = 235,
|
||||
k_param_pid_rate_pitch,
|
||||
k_param_pid_rate_yaw,
|
||||
k_param_pi_stabilize_roll,
|
||||
k_param_pi_stabilize_pitch,
|
||||
k_param_pi_stabilize_yaw,
|
||||
k_param_pi_loiter_lat,
|
||||
k_param_pi_loiter_lon,
|
||||
k_param_pi_nav_lat,
|
||||
k_param_pi_nav_lon,
|
||||
k_param_pid_nav_lat,
|
||||
k_param_pid_nav_lon,
|
||||
k_param_pi_alt_hold,
|
||||
k_param_pi_throttle,
|
||||
k_param_pi_acro_roll,
|
||||
k_param_pi_acro_pitch,
|
||||
k_param_pi_optflow_roll,
|
||||
k_param_pi_optflow_pitch, // 250
|
||||
k_param_loiter_d,
|
||||
k_param_pid_throttle,
|
||||
k_param_pid_optflow_roll,
|
||||
k_param_pid_optflow_pitch, // 250
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
@ -286,31 +282,24 @@ public:
|
||||
AP_Float camera_pitch_gain;
|
||||
AP_Float camera_roll_gain;
|
||||
AP_Float stablize_d;
|
||||
AP_Float loiter_d;
|
||||
|
||||
// PI/D controllers
|
||||
APM_PI pi_rate_roll;
|
||||
APM_PI pi_rate_pitch;
|
||||
APM_PI pi_rate_yaw;
|
||||
AC_PID pid_rate_roll;
|
||||
AC_PID pid_rate_pitch;
|
||||
AC_PID pid_rate_yaw;
|
||||
AC_PID pid_nav_lat;
|
||||
AC_PID pid_nav_lon;
|
||||
|
||||
APM_PI pi_stabilize_roll;
|
||||
APM_PI pi_stabilize_pitch;
|
||||
APM_PI pi_stabilize_yaw;
|
||||
AC_PID pid_throttle;
|
||||
AC_PID pid_optflow_roll;
|
||||
AC_PID pid_optflow_pitch;
|
||||
|
||||
APM_PI pi_loiter_lat;
|
||||
APM_PI pi_loiter_lon;
|
||||
|
||||
APM_PI pi_nav_lat;
|
||||
APM_PI pi_nav_lon;
|
||||
|
||||
APM_PI pi_stabilize_roll;
|
||||
APM_PI pi_stabilize_pitch;
|
||||
APM_PI pi_stabilize_yaw;
|
||||
APM_PI pi_alt_hold;
|
||||
APM_PI pi_throttle;
|
||||
|
||||
APM_PI pi_acro_roll;
|
||||
APM_PI pi_acro_pitch;
|
||||
|
||||
PID pi_optflow_roll;
|
||||
PID pi_optflow_pitch;
|
||||
|
||||
uint8_t junk;
|
||||
|
||||
@ -369,7 +358,7 @@ public:
|
||||
radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")),
|
||||
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
|
||||
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),
|
||||
ch7_option (CH7_SAVE_WP, k_param_ch7_option, PSTR("CH7_OPT")),
|
||||
ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")),
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
|
||||
@ -409,34 +398,32 @@ public:
|
||||
//-------------------------------------------------------------------------------------------------------------------
|
||||
camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")),
|
||||
camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")),
|
||||
|
||||
stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")),
|
||||
loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")),
|
||||
|
||||
// PID controller group key name initial P initial I initial D initial imax
|
||||
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100),
|
||||
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100),
|
||||
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100),
|
||||
|
||||
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
|
||||
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
|
||||
|
||||
pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
|
||||
pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_IMAX * 100),
|
||||
pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100),
|
||||
|
||||
|
||||
// PI controller group key name initial P initial I initial imax
|
||||
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100),
|
||||
pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100),
|
||||
pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100),
|
||||
|
||||
pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
|
||||
pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
|
||||
pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
|
||||
|
||||
pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX),
|
||||
pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
|
||||
pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100),
|
||||
|
||||
pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100),
|
||||
pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100),
|
||||
|
||||
pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX),
|
||||
pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX),
|
||||
|
||||
pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100),
|
||||
pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100),
|
||||
|
||||
pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100),
|
||||
pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100),
|
||||
|
||||
junk(0) // XXX just so that we can add things without worrying about the trailing comma
|
||||
{
|
||||
|
||||
@ -16,6 +16,7 @@ static void process_nav_command()
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
|
||||
yaw_mode = YAW_HOLD;
|
||||
do_land();
|
||||
break;
|
||||
|
||||
@ -121,8 +122,6 @@ static void process_now_command()
|
||||
|
||||
static bool verify_must()
|
||||
{
|
||||
//Serial.printf("vmust: %d\n", command_nav_ID);
|
||||
|
||||
switch(command_nav_queue.id) {
|
||||
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
@ -130,7 +129,11 @@ static bool verify_must()
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LAND:
|
||||
return verify_land();
|
||||
if(g.sonar_enabled == true){
|
||||
return verify_land_sonar();
|
||||
}else{
|
||||
return verify_land_baro();
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
@ -265,13 +268,21 @@ static void do_land()
|
||||
{
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
// just to make sure
|
||||
land_complete = false;
|
||||
|
||||
// landing boost lowers the main throttle to mimmick
|
||||
// the effect of a user's hand
|
||||
landing_boost = 0;
|
||||
|
||||
// A counter that goes up if our climb rate stalls out.
|
||||
ground_detector = 0;
|
||||
|
||||
// hold at our current location
|
||||
set_next_WP(¤t_loc);
|
||||
|
||||
// Set a new target altitude
|
||||
set_new_altitude(-200);
|
||||
// Set a new target altitude very low, incase we are landing on a hill!
|
||||
set_new_altitude(-1000);
|
||||
}
|
||||
|
||||
static void do_loiter_unlimited()
|
||||
@ -341,46 +352,71 @@ static bool verify_takeoff()
|
||||
}
|
||||
|
||||
// called at 10hz
|
||||
static bool verify_land()
|
||||
static bool verify_land_sonar()
|
||||
{
|
||||
static int16_t velocity_land = -1;
|
||||
static float icount = 0;
|
||||
static float icount = 1;
|
||||
|
||||
// landing detector
|
||||
if (current_loc.alt > 300){
|
||||
velocity_land = 2000;
|
||||
if(current_loc.alt > 300){
|
||||
wp_control = LOITER_MODE;
|
||||
icount = 1;
|
||||
|
||||
ground_detector = 0;
|
||||
}else{
|
||||
// a LP filter used to tell if we have landed
|
||||
// will drive to 0 if we are on the ground - maybe, the baro is noisy
|
||||
velocity_land = ((velocity_land * 7) + climb_rate ) / 8;
|
||||
// begin to pull down on the throttle
|
||||
landing_boost++;
|
||||
}
|
||||
|
||||
if ((current_loc.alt - home.alt) < 200){
|
||||
// don't bank to hold position
|
||||
wp_control = NO_NAV_MODE;
|
||||
// try and come down faster
|
||||
//landing_boost++;
|
||||
//landing_boost = min(landing_boost, 15);
|
||||
float tmp = (1.75 * icount * icount) - (7.2 * icount);
|
||||
landing_boost = tmp;
|
||||
landing_boost = constrain(landing_boost, 1, 200);
|
||||
icount += 0.4;
|
||||
|
||||
//Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount);
|
||||
|
||||
}else{
|
||||
landing_boost = 0;
|
||||
wp_control = LOITER_MODE;
|
||||
if(current_loc.alt < 200 ){
|
||||
wp_control = NO_NAV_MODE;
|
||||
}
|
||||
|
||||
//if(/*(current_loc.alt < 100) && */ (velocity_land < 20)){
|
||||
if((landing_boost > 150) || (velocity_land < 20)){
|
||||
icount = 1;
|
||||
land_complete = true;
|
||||
landing_boost = 0;
|
||||
return true;
|
||||
if(current_loc.alt < 150 ){
|
||||
//rapid throttle reduction
|
||||
int16_t lb = (1.75 * icount * icount) - (7.2 * icount);
|
||||
icount++;
|
||||
lb = constrain(lb, 0, 180);
|
||||
landing_boost += lb;
|
||||
//Serial.printf("%0.0f, %d, %d, %d\n", icount, current_loc.alt, landing_boost, lb);
|
||||
|
||||
if(current_loc.alt < 40 || abs(climb_rate) < 20) {
|
||||
if(ground_detector++ > 20) {
|
||||
land_complete = true;
|
||||
ground_detector = 0;
|
||||
icount = 1;
|
||||
// init disarm motors
|
||||
init_disarm_motors();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_land_baro()
|
||||
{
|
||||
if(current_loc.alt > 300){
|
||||
wp_control = LOITER_MODE;
|
||||
ground_detector = 0;
|
||||
}else{
|
||||
// begin to pull down on the throttle
|
||||
landing_boost++;
|
||||
landing_boost = min(landing_boost, 40);
|
||||
}
|
||||
if(current_loc.alt < 200 ){
|
||||
wp_control = NO_NAV_MODE;
|
||||
}
|
||||
|
||||
if(current_loc.alt < 150 ){
|
||||
if(abs(climb_rate) < 20) {
|
||||
if(ground_detector++ > 30) {
|
||||
//landing_boost = 100;
|
||||
//Serial.print("land_complete\n");
|
||||
land_complete = true;
|
||||
ground_detector = 0;
|
||||
// init disarm motors
|
||||
init_disarm_motors();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -62,14 +62,7 @@ static void update_commands()
|
||||
g.command_index = command_nav_index = 255;
|
||||
// if we are on the ground, enter stabilize, else Land
|
||||
if (land_complete == true){
|
||||
// So what state does this leave us in?
|
||||
// We are still in the same mode as what landed us,
|
||||
// so maybe we try to continue to descend just in case we are still in the air
|
||||
// This will also drive down the Iterm to -300
|
||||
|
||||
// We can't disarm the motors easily. We could very well be wrong
|
||||
//
|
||||
//init_disarm_motors();
|
||||
// we will disarm the motors after landing.
|
||||
} else {
|
||||
set_mode(LAND);
|
||||
}
|
||||
|
||||
@ -525,114 +525,98 @@
|
||||
#ifdef MOTORS_JD880
|
||||
# define STABILIZE_ROLL_P 3.6
|
||||
# define STABILIZE_ROLL_I 0.0
|
||||
# define STABILIZE_ROLL_IMAX 40.0 // degrees
|
||||
# define STABILIZE_ROLL_IMAX 40.0 // degrees
|
||||
# define STABILIZE_PITCH_P 3.6
|
||||
# define STABILIZE_PITCH_I 0.0
|
||||
# define STABILIZE_PITCH_IMAX 40.0 // degrees
|
||||
# define STABILIZE_PITCH_IMAX 40.0 // degrees
|
||||
#endif
|
||||
|
||||
#ifdef MOTORS_JD850
|
||||
# define STABILIZE_ROLL_P 4.0
|
||||
# define STABILIZE_ROLL_I 0.0
|
||||
# define STABILIZE_ROLL_IMAX 40.0 // degrees
|
||||
# define STABILIZE_ROLL_IMAX 40.0 // degrees
|
||||
# define STABILIZE_PITCH_P 4.0
|
||||
# define STABILIZE_PITCH_I 0.0
|
||||
# define STABILIZE_PITCH_IMAX 40.0 // degrees
|
||||
# define STABILIZE_PITCH_IMAX 40.0 // degrees
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef STABILIZE_D
|
||||
# define STABILIZE_D .03
|
||||
# define STABILIZE_D .12
|
||||
#endif
|
||||
|
||||
// Jasons default values that are good for smaller payload motors.
|
||||
#ifndef STABILIZE_ROLL_P
|
||||
# define STABILIZE_ROLL_P 4.6
|
||||
# define STABILIZE_ROLL_P 4.5
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_I
|
||||
# define STABILIZE_ROLL_I 0.1
|
||||
# define STABILIZE_ROLL_I 0.0
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_IMAX
|
||||
# define STABILIZE_ROLL_IMAX 40 // degrees
|
||||
#endif
|
||||
|
||||
#ifndef STABILIZE_PITCH_P
|
||||
# define STABILIZE_PITCH_P 4.6
|
||||
# define STABILIZE_PITCH_P 4.5
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_I
|
||||
# define STABILIZE_PITCH_I 0.1
|
||||
# define STABILIZE_PITCH_I 0.0
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_IMAX
|
||||
# define STABILIZE_PITCH_IMAX 40 // degrees
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Acro Rate Control
|
||||
//
|
||||
#ifndef ACRO_ROLL_P
|
||||
# define ACRO_ROLL_P 0.155
|
||||
#ifndef STABILIZE_YAW_P
|
||||
# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
|
||||
#endif
|
||||
#ifndef ACRO_ROLL_I
|
||||
# define ACRO_ROLL_I 0.0
|
||||
#ifndef STABILIZE_YAW_I
|
||||
# define STABILIZE_YAW_I 0.01
|
||||
#endif
|
||||
#ifndef ACRO_ROLL_IMAX
|
||||
# define ACRO_ROLL_IMAX 15 // degrees
|
||||
#ifndef STABILIZE_YAW_IMAX
|
||||
# define STABILIZE_YAW_IMAX 8 // degrees * 100
|
||||
#endif
|
||||
|
||||
#ifndef ACRO_PITCH_P
|
||||
# define ACRO_PITCH_P 0.155
|
||||
#endif
|
||||
#ifndef ACRO_PITCH_I
|
||||
# define ACRO_PITCH_I 0 //0.18
|
||||
#endif
|
||||
#ifndef ACRO_PITCH_IMAX
|
||||
# define ACRO_PITCH_IMAX 15 // degrees
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Stabilize Rate Control
|
||||
//
|
||||
#ifndef RATE_ROLL_P
|
||||
# define RATE_ROLL_P 0.155
|
||||
# define RATE_ROLL_P 0.14
|
||||
#endif
|
||||
#ifndef RATE_ROLL_I
|
||||
# define RATE_ROLL_I 0.0
|
||||
# define RATE_ROLL_I 0.0
|
||||
#endif
|
||||
#ifndef RATE_ROLL_D
|
||||
# define RATE_ROLL_D 0
|
||||
#endif
|
||||
#ifndef RATE_ROLL_IMAX
|
||||
# define RATE_ROLL_IMAX 15 // degrees
|
||||
#endif
|
||||
|
||||
#ifndef RATE_PITCH_P
|
||||
# define RATE_PITCH_P 0.155
|
||||
# define RATE_PITCH_P 0.14
|
||||
#endif
|
||||
#ifndef RATE_PITCH_I
|
||||
# define RATE_PITCH_I 0 //0.18
|
||||
# define RATE_PITCH_I 0 // 0.18
|
||||
#endif
|
||||
#ifndef RATE_PITCH_D
|
||||
# define RATE_PITCH_D 0 // 0.002
|
||||
#endif
|
||||
#ifndef RATE_PITCH_IMAX
|
||||
# define RATE_PITCH_IMAX 15 // degrees
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// YAW Control
|
||||
//
|
||||
#ifndef STABILIZE_YAW_P
|
||||
# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
|
||||
#endif
|
||||
#ifndef STABILIZE_YAW_I
|
||||
# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
|
||||
#endif
|
||||
#ifndef STABILIZE_YAW_IMAX
|
||||
# define STABILIZE_YAW_IMAX 8 // degrees * 100
|
||||
#endif
|
||||
|
||||
#ifndef RATE_YAW_P
|
||||
# define RATE_YAW_P .13 // used to control response in turning
|
||||
# define RATE_YAW_P .13
|
||||
#endif
|
||||
#ifndef RATE_YAW_I
|
||||
# define RATE_YAW_I 0.0
|
||||
# define RATE_YAW_I 0.0
|
||||
#endif
|
||||
#ifndef RATE_YAW_D
|
||||
# define RATE_YAW_D .002
|
||||
#endif
|
||||
#ifndef RATE_YAW_IMAX
|
||||
# define RATE_YAW_IMAX 50
|
||||
# define RATE_YAW_IMAX 50 // degrees
|
||||
#endif
|
||||
|
||||
|
||||
@ -640,37 +624,37 @@
|
||||
// Loiter control gains
|
||||
//
|
||||
#ifndef LOITER_P
|
||||
# define LOITER_P 1.5 // was .25 in previous
|
||||
# define LOITER_P .4 // was .25 in previous
|
||||
#endif
|
||||
#ifndef LOITER_I
|
||||
# define LOITER_I 0.09 // Wind control
|
||||
# define LOITER_I 0.2 // Wind control
|
||||
#endif
|
||||
#ifndef LOITER_IMAX
|
||||
# define LOITER_IMAX 30 // degrees°
|
||||
#endif
|
||||
#ifndef LOITER_D
|
||||
# define LOITER_D 2.2 // rate control
|
||||
# define LOITER_IMAX 30 // degrees
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// WP Navigation control gains
|
||||
//
|
||||
#ifndef NAV_P
|
||||
# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter
|
||||
# define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter
|
||||
#endif
|
||||
#ifndef NAV_I
|
||||
# define NAV_I 0 //
|
||||
#endif
|
||||
#ifndef NAV_D
|
||||
# define NAV_D 0.015 //
|
||||
#endif
|
||||
#ifndef NAV_IMAX
|
||||
# define NAV_IMAX 30 // degrees
|
||||
#endif
|
||||
|
||||
#ifndef WAYPOINT_SPEED_MAX
|
||||
# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph
|
||||
# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph
|
||||
#endif
|
||||
|
||||
#ifndef WAYPOINT_SPEED_MIN
|
||||
# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph
|
||||
# define WAYPOINT_SPEED_MIN 100 // 1m/s
|
||||
#endif
|
||||
|
||||
|
||||
@ -678,28 +662,31 @@
|
||||
// Throttle control gains
|
||||
//
|
||||
#ifndef THROTTLE_CRUISE
|
||||
# define THROTTLE_CRUISE 350 //
|
||||
# define THROTTLE_CRUISE 350 //
|
||||
#endif
|
||||
|
||||
#ifndef THR_HOLD_P
|
||||
# define THR_HOLD_P 0.4 //
|
||||
#ifndef ALT_HOLD_P
|
||||
# define ALT_HOLD_P 0.4 //
|
||||
#endif
|
||||
#ifndef THR_HOLD_I
|
||||
# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup
|
||||
#ifndef ALT_HOLD_I
|
||||
# define ALT_HOLD_I 0.02
|
||||
#endif
|
||||
#ifndef THR_HOLD_IMAX
|
||||
# define THR_HOLD_IMAX 300
|
||||
#ifndef ALT_HOLD_IMAX
|
||||
# define ALT_HOLD_IMAX 300
|
||||
#endif
|
||||
|
||||
// RATE control
|
||||
#ifndef THROTTLE_P
|
||||
# define THROTTLE_P 0.5 //
|
||||
# define THROTTLE_P 0.5 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.0 //
|
||||
# define THROTTLE_I 0.0 //
|
||||
#endif
|
||||
#ifndef THROTTLE_D
|
||||
# define THROTTLE_D 0.02 //
|
||||
#endif
|
||||
#ifndef THROTTLE_IMAX
|
||||
# define THROTTLE_IMAX 300
|
||||
# define THROTTLE_IMAX 300
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -114,6 +114,7 @@ static void read_trim_switch()
|
||||
// reset the mission
|
||||
CH7_wp_index = 0;
|
||||
g.command_total.set_and_save(1);
|
||||
set_mode(RTL);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -7,13 +7,11 @@ static byte navigate()
|
||||
{
|
||||
// waypoint distance from plane in meters
|
||||
// ---------------------------------------
|
||||
wp_distance = get_distance(¤t_loc, &next_WP);
|
||||
home_distance = get_distance(¤t_loc, &home);
|
||||
wp_distance = get_distance(¤t_loc, &next_WP);
|
||||
home_distance = get_distance(¤t_loc, &home);
|
||||
|
||||
if (wp_distance < 0){
|
||||
//gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
|
||||
//Serial.println(wp_distance,DEC);
|
||||
//print_current_waypoints();
|
||||
// something went very wrong
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -61,9 +59,8 @@ static void calc_XY_velocity(){
|
||||
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp;
|
||||
|
||||
// slight averaging filter
|
||||
x_GPS_speed = (x_GPS_speed + x_estimate) >> 1;
|
||||
y_GPS_speed = (y_GPS_speed + y_estimate) >> 1;
|
||||
//*/
|
||||
x_actual_speed = (x_actual_speed + x_estimate) >> 1;
|
||||
y_actual_speed = (y_actual_speed + y_estimate) >> 1;
|
||||
|
||||
/*
|
||||
// Ryan Beall's forward estimator:
|
||||
@ -99,67 +96,7 @@ static void calc_location_error(struct Location *next_loc)
|
||||
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
|
||||
}
|
||||
|
||||
/*
|
||||
//static void calc_loiter3(int x_error, int y_error)
|
||||
{
|
||||
static int32_t gps_lat_I = 0;
|
||||
static int32_t gps_lon_I = 0;
|
||||
|
||||
// If close to goal <1m reset the I term
|
||||
if (abs(x_error) < 50)
|
||||
gps_lon_I = 0;
|
||||
if (abs(y_error) < 50)
|
||||
gps_lat_I = 0;
|
||||
|
||||
gps_lon_I += x_error;
|
||||
gps_lat_I += y_error;
|
||||
|
||||
gps_lon_I = constrain(gps_lon_I,-3000,3000);
|
||||
gps_lat_I = constrain(gps_lat_I,-3000,3000);
|
||||
|
||||
int16_t lon_P = 1.2 * (float)x_error;
|
||||
int16_t lon_I = 0.1 * (float)gps_lon_I; //.1
|
||||
int16_t lon_D = 3 * x_GPS_speed ; // this controls the small bumps
|
||||
|
||||
int16_t lat_P = 1.2 * (float)y_error;
|
||||
int16_t lat_I = 0.1 * (float)gps_lat_I;
|
||||
int16_t lat_D = 3 * y_GPS_speed ;
|
||||
|
||||
//limit of terms
|
||||
lon_I = constrain(lon_I,-3000,3000);
|
||||
lat_I = constrain(lat_I,-3000,3000);
|
||||
lon_D = constrain(lon_D,-500,500); //this controls the long distance dampimg
|
||||
lat_D = constrain(lat_D,-500,500); //this controls the long distance dampimg
|
||||
|
||||
nav_lon = lon_P + lon_I - lon_D;
|
||||
nav_lat = lat_P + lat_I - lat_D;
|
||||
|
||||
Serial.printf("%d, %d, %d, %d, %d, %d\n",
|
||||
lon_P, lat_P,
|
||||
lon_I, lat_I,
|
||||
lon_D, lat_D);
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
#define NAV_ERR_MAX 800
|
||||
static void calc_loiter1(int x_error, int y_error)
|
||||
{
|
||||
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
|
||||
int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps
|
||||
|
||||
int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav);
|
||||
int16_t lat_D = g.loiter_d * y_actual_speed ;
|
||||
|
||||
//limit of terms
|
||||
lon_D = constrain(lon_D,-500,500);
|
||||
lat_D = constrain(lat_D,-500,500);
|
||||
|
||||
nav_lon = constrain(lon_PI - lon_D, -2500, 2500);
|
||||
nav_lat = constrain(lat_PI - lat_D, -2500, 2500);
|
||||
}
|
||||
|
||||
|
||||
static void calc_loiter(int x_error, int y_error)
|
||||
{
|
||||
// East/West
|
||||
@ -167,22 +104,20 @@ static void calc_loiter(int x_error, int y_error)
|
||||
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
|
||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
|
||||
x_rate_error = x_target_speed - x_actual_speed;
|
||||
nav_lon_p = x_rate_error * g.loiter_d;
|
||||
|
||||
nav_lon_p = constrain(nav_lon_p, -1200, 1200);
|
||||
nav_lon = nav_lon_p + x_iterm;
|
||||
nav_lon = constrain(nav_lon, -2500, 2500);
|
||||
|
||||
// North/South
|
||||
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
|
||||
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
|
||||
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
|
||||
y_rate_error = y_target_speed - y_actual_speed;
|
||||
nav_lat_p = y_rate_error * g.loiter_d;
|
||||
|
||||
nav_lat_p = constrain(nav_lat_p, -1200, 1200);
|
||||
nav_lat = nav_lat_p + y_iterm;
|
||||
nav_lat = constrain(nav_lat, -2500, 2500);
|
||||
calc_nav_lon(x_rate_error);
|
||||
calc_nav_lat(y_rate_error);
|
||||
|
||||
nav_lat = nav_lat + y_iterm;
|
||||
nav_lon = nav_lon + x_iterm;
|
||||
|
||||
|
||||
|
||||
/*
|
||||
int8_t ttt = 1.0/dTnav;
|
||||
@ -204,7 +139,7 @@ static void calc_loiter(int x_error, int y_error)
|
||||
//*/
|
||||
|
||||
/*
|
||||
int16_t t1 = g.pi_nav_lon.get_integrator(); // X
|
||||
int16_t t1 = g.pid_nav_lon.get_integrator(); // X
|
||||
Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n",
|
||||
wp_distance, //1
|
||||
dTnav, //2
|
||||
@ -219,33 +154,91 @@ static void calc_loiter(int x_error, int y_error)
|
||||
//*/
|
||||
}
|
||||
|
||||
static void calc_nav_rate(int max_speed)
|
||||
{
|
||||
// push us towards the original track
|
||||
update_crosstrack();
|
||||
|
||||
// nav_bearing includes crosstrack
|
||||
float temp = (9000l - nav_bearing) * RADX100;
|
||||
|
||||
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
|
||||
x_rate_error = constrain(x_rate_error, -1000, 1000);
|
||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
|
||||
|
||||
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
|
||||
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
|
||||
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
|
||||
|
||||
calc_nav_lon(x_rate_error);
|
||||
calc_nav_lat(y_rate_error);
|
||||
|
||||
nav_lon = nav_lon + x_iterm;
|
||||
nav_lat = nav_lat + y_iterm;
|
||||
|
||||
/*
|
||||
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat,
|
||||
x_iterm,
|
||||
y_iterm,
|
||||
crosstrack_error);
|
||||
//*/
|
||||
|
||||
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
|
||||
|
||||
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat);*/
|
||||
}
|
||||
|
||||
|
||||
static void calc_nav_lon(int rate)
|
||||
{
|
||||
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
|
||||
nav_lon = get_corrected_angle(rate, nav_lon);
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
}
|
||||
|
||||
static void calc_nav_lat(int rate)
|
||||
{
|
||||
nav_lat = g.pid_nav_lon.get_pid(rate, dTnav);
|
||||
nav_lat = get_corrected_angle(rate, nav_lat);
|
||||
nav_lat = constrain(nav_lat, -3000, 3000);
|
||||
}
|
||||
|
||||
static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
|
||||
{
|
||||
int16_t tt = desired_rate;
|
||||
// scale down the desired rate and square it
|
||||
desired_rate = desired_rate / 20;
|
||||
desired_rate = desired_rate * desired_rate;
|
||||
int16_t tmp = 0;
|
||||
|
||||
if (tt > 0){
|
||||
tmp = rate_out + (rate_out - desired_rate);
|
||||
tmp = max(tmp, rate_out);
|
||||
}else if (tt < 0){
|
||||
tmp = rate_out + (rate_out + desired_rate);
|
||||
tmp = min(tmp, rate_out);
|
||||
}
|
||||
//Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
//wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2
|
||||
|
||||
|
||||
#define ERR_GAIN .01
|
||||
// called at 50hz
|
||||
static void estimate_velocity()
|
||||
{
|
||||
// we need to extimate velocity when below GPS threshold of 1.5m/s
|
||||
//if(g_gps->ground_speed < 120){
|
||||
// some smoothing to prevent bumpy rides
|
||||
x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16;
|
||||
y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16;
|
||||
|
||||
// integration of nav_p angle
|
||||
//x_actual_speed += (nav_lon_p >>2);
|
||||
//y_actual_speed += (nav_lat_p >>2);
|
||||
|
||||
// this is just what worked best in SIM
|
||||
//x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4;
|
||||
//y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4;
|
||||
|
||||
//}else{
|
||||
// less smoothing needed since the GPS already filters
|
||||
// x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
|
||||
// y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
|
||||
//}
|
||||
}
|
||||
|
||||
// this calculation rotates our World frame of reference to the copter's frame of reference
|
||||
// We use the DCM's matrix to precalculate these trig values at 50hz
|
||||
@ -291,55 +284,6 @@ static int16_t calc_desired_speed(int16_t max_speed)
|
||||
return max_speed;
|
||||
}
|
||||
|
||||
static void calc_nav_rate(int max_speed)
|
||||
{
|
||||
// push us towards the original track
|
||||
update_crosstrack();
|
||||
|
||||
// nav_bearing includes crosstrack
|
||||
float temp = (9000l - nav_bearing) * RADX100;
|
||||
|
||||
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
|
||||
x_rate_error = constrain(x_rate_error, -1000, 1000);
|
||||
int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav);
|
||||
nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
|
||||
nav_lon = nav_lon_p + x_iterm;
|
||||
nav_lon = constrain(nav_lon, -3000, 3000);
|
||||
|
||||
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413
|
||||
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
|
||||
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
|
||||
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
|
||||
nav_lat = nav_lat_p + y_iterm;
|
||||
nav_lat = constrain(nav_lat, -3000, 3000);
|
||||
|
||||
/*
|
||||
Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat,
|
||||
x_iterm,
|
||||
y_iterm,
|
||||
crosstrack_error);
|
||||
//*/
|
||||
|
||||
|
||||
// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
|
||||
|
||||
/*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ",
|
||||
max_speed,
|
||||
x_actual_speed,
|
||||
y_actual_speed,
|
||||
x_rate_error,
|
||||
y_rate_error,
|
||||
nav_lon,
|
||||
nav_lat);*/
|
||||
}
|
||||
|
||||
|
||||
static void update_crosstrack(void)
|
||||
{
|
||||
@ -433,7 +377,7 @@ static int32_t get_new_altitude()
|
||||
}
|
||||
|
||||
int32_t diff = abs(next_WP.alt - target_altitude);
|
||||
int8_t _scale = 3;
|
||||
int8_t _scale = 4;
|
||||
|
||||
if (next_WP.alt < target_altitude){
|
||||
// we are below the target alt
|
||||
@ -444,12 +388,12 @@ static int32_t get_new_altitude()
|
||||
}
|
||||
}else {
|
||||
// we are above the target, going down
|
||||
if(diff < 600){
|
||||
_scale = 4;
|
||||
}
|
||||
if(diff < 300){
|
||||
if(diff < 400){
|
||||
_scale = 5;
|
||||
}
|
||||
if(diff < 100){
|
||||
_scale = 6;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t change = (millis() - alt_change_timer) >> _scale;
|
||||
|
||||
@ -354,8 +354,8 @@ static void init_ardupilot()
|
||||
Log_Write_Startup();
|
||||
Log_Write_Data(10, g.pi_stabilize_roll.kP());
|
||||
Log_Write_Data(11, g.pi_stabilize_pitch.kP());
|
||||
Log_Write_Data(12, g.pi_rate_roll.kP());
|
||||
Log_Write_Data(13, g.pi_rate_pitch.kP());
|
||||
Log_Write_Data(12, g.pid_rate_roll.kP());
|
||||
Log_Write_Data(13, g.pid_rate_pitch.kP());
|
||||
#endif
|
||||
|
||||
SendDebug("\nReady to FLY ");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user