mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
Conflicts: ArduCopter/APM_Config.h ArduPlane/APM_Config.h
This commit is contained in:
Chris Anderson
commit
ca201a7d9d
@ -2,6 +2,9 @@
|
||||
|
||||
// Example config file. Take a look at config.h. Any term define there can be overridden by defining it here.
|
||||
|
||||
// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// # define APM2_BETA_HARDWARE
|
||||
|
||||
// GPS is auto-selected
|
||||
|
||||
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
@ -27,7 +30,7 @@
|
||||
V_FRAME
|
||||
*/
|
||||
|
||||
# define CH7_OPTION CH7_SAVE_WP
|
||||
//# define CH7_OPTION CH7_SAVE_WP
|
||||
/*
|
||||
CH7_DO_NOTHING
|
||||
CH7_SET_HOVER
|
||||
@ -41,9 +44,6 @@
|
||||
|
||||
#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress
|
||||
|
||||
// lets use Manual throttle during Loiter
|
||||
//#define LOITER_THR THROTTLE_MANUAL
|
||||
# define RTL_YAW YAW_HOLD
|
||||
|
||||
//#define RATE_ROLL_I 0.18
|
||||
//#define RATE_PITCH_I 0.18
|
||||
@ -70,6 +70,4 @@
|
||||
// #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// #define APM2_BETA_HARDWARE // for developers who received an early beta board with the older BMP085
|
||||
|
||||
|
||||
// This is experimental!!, be caureful, effects stable mode
|
||||
#define WIND_COMP_STAB 0
|
||||
//# define LOGGING_ENABLED DISABLED
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r7 alpha"
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r8 alpha"
|
||||
/*
|
||||
ArduCopter Version 2.0 Beta
|
||||
Authors: Jason Short
|
||||
@ -665,6 +665,11 @@ static int16_t nav_lon;
|
||||
static int16_t nav_lat_p;
|
||||
static int16_t nav_lon_p;
|
||||
|
||||
// The Commanded ROll from the autopilot based on optical flow sensor.
|
||||
static int32_t of_roll = 0;
|
||||
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
|
||||
static int32_t of_pitch = 0;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation Throttle control
|
||||
@ -926,12 +931,6 @@ static void medium_loop()
|
||||
update_GPS();
|
||||
}
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
if(g.optflow_enabled){
|
||||
update_optical_flow();
|
||||
}
|
||||
#endif
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
|
||||
if(g.compass_enabled){
|
||||
if (compass.read()) {
|
||||
@ -975,13 +974,13 @@ static void medium_loop()
|
||||
// If we have optFlow enabled we can grab a more accurate speed
|
||||
// here and override the speed from the GPS
|
||||
// ----------------------------------------
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
if(g.optflow_enabled && current_loc.alt < 500){
|
||||
// optflow wont be enabled on 1280's
|
||||
x_GPS_speed = optflow.x_cm;
|
||||
y_GPS_speed = optflow.y_cm;
|
||||
}
|
||||
#endif
|
||||
//#ifdef OPTFLOW_ENABLED
|
||||
//if(g.optflow_enabled && current_loc.alt < 500){
|
||||
// // optflow wont be enabled on 1280's
|
||||
// x_GPS_speed = optflow.x_cm;
|
||||
// y_GPS_speed = optflow.y_cm;
|
||||
//}
|
||||
//#endif
|
||||
|
||||
// control mode specific updates
|
||||
// -----------------------------
|
||||
@ -1096,6 +1095,13 @@ static void fifty_hz_loop()
|
||||
}
|
||||
#endif
|
||||
|
||||
// syncronise optical flow reads with altitude reads
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
if(g.optflow_enabled){
|
||||
update_optical_flow();
|
||||
}
|
||||
#endif
|
||||
|
||||
// agmatthews - USERHOOKS
|
||||
#ifdef USERHOOK_50HZLOOP
|
||||
USERHOOK_50HZLOOP
|
||||
@ -1222,15 +1228,21 @@ static void super_slow_loop()
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
static void update_optical_flow(void)
|
||||
{
|
||||
static int log_counter = 0;
|
||||
|
||||
optflow.update();
|
||||
optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
|
||||
|
||||
// write to log
|
||||
if (g.log_bitmask & MASK_LOG_OPTFLOW){
|
||||
Log_Write_Optflow();
|
||||
log_counter++;
|
||||
if( log_counter >= 5 ) {
|
||||
log_counter = 0;
|
||||
if (g.log_bitmask & MASK_LOG_OPTFLOW){
|
||||
Log_Write_Optflow();
|
||||
}
|
||||
}
|
||||
|
||||
if(g.optflow_enabled && current_loc.alt < 500){
|
||||
/*if(g.optflow_enabled && current_loc.alt < 500){
|
||||
if(GPS_enabled){
|
||||
// if we have a GPS, we add some detail to the GPS
|
||||
// XXX this may not ne right
|
||||
@ -1247,7 +1259,7 @@ static void update_optical_flow(void)
|
||||
}
|
||||
// OK to run the nav routines
|
||||
nav_ok = true;
|
||||
}
|
||||
}*/
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1390,15 +1402,17 @@ void update_roll_pitch_mode(void)
|
||||
if(do_simple && new_radio_frame){
|
||||
update_simple_mode();
|
||||
}
|
||||
|
||||
#if WIND_COMP_STAB == 1
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
|
||||
#else
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_AUTO:
|
||||
@ -1412,6 +1426,22 @@ void update_roll_pitch_mode(void)
|
||||
g.rc_1.servo_out = get_stabilize_roll(control_roll);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_STABLE_OF:
|
||||
// apply SIMPLE mode transform
|
||||
if(do_simple && new_radio_frame){
|
||||
update_simple_mode();
|
||||
}
|
||||
|
||||
// in this mode, nav_roll and nav_pitch = the iterm
|
||||
#if WIND_COMP_STAB == 1
|
||||
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in + nav_roll));
|
||||
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in + nav_pitch));
|
||||
#else
|
||||
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in));
|
||||
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in));
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
// clear new radio frame info
|
||||
@ -1612,6 +1642,10 @@ static void update_navigation()
|
||||
set_mode(LOITER);
|
||||
auto_land_timer = millis();
|
||||
|
||||
}else if(current_loc.alt < (next_WP.alt - 300)){
|
||||
// don't navigate if we are below our target alt
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
}else{
|
||||
// calculates desired Yaw
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
@ -1655,10 +1689,13 @@ static void update_navigation()
|
||||
case LAND:
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
if (current_loc.alt < 250){
|
||||
wp_control = NO_NAV_MODE;
|
||||
next_WP.alt = -200; // force us down
|
||||
if(verify_land()) { // JLN fix for auto land in RTL
|
||||
set_mode(STABILIZE);
|
||||
} else {
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
}
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
break;
|
||||
@ -1877,6 +1914,8 @@ static void tuning(){
|
||||
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);
|
||||
break;
|
||||
|
||||
case CH6_RATE_KI:
|
||||
@ -1939,6 +1978,19 @@ static void tuning(){
|
||||
g.rc_6.set_range(0,1000); // 0 to 1
|
||||
g.pi_alt_hold.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_OPTFLOW_KP:
|
||||
g.rc_6.set_range(0,10000); // 0 to 10
|
||||
g.pi_optflow_roll.kP(tuning_value);
|
||||
g.pi_optflow_pitch.kP(tuning_value);
|
||||
break;
|
||||
|
||||
case CH6_OPTFLOW_KI:
|
||||
g.rc_6.set_range(0,100); // 0 to 0.1
|
||||
g.pi_optflow_roll.kI(tuning_value);
|
||||
g.pi_optflow_pitch.kI(tuning_value);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -2012,14 +2064,19 @@ static void update_nav_wp()
|
||||
|
||||
}else if(wp_control == NO_NAV_MODE){
|
||||
// calc the Iterms for Loiter based on velocity
|
||||
//if ((x_actual_speed + y_actual_speed) == 0)
|
||||
if (g_gps->ground_speed < 50)
|
||||
calc_wind_compensation();
|
||||
else
|
||||
reduce_wind_compensation();
|
||||
#if WIND_COMP_STAB == 1
|
||||
if (g_gps->ground_speed < 50)
|
||||
calc_wind_compensation();
|
||||
else
|
||||
reduce_wind_compensation();
|
||||
|
||||
// rotate nav_lat, nav_lon to roll and pitch
|
||||
calc_loiter_pitch_roll();
|
||||
// rotate nav_lat, nav_lon to roll and pitch
|
||||
calc_loiter_pitch_roll();
|
||||
#else
|
||||
// clear out our nav so we can do things like land straight
|
||||
nav_pitch = 0;
|
||||
nav_roll = 0;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -258,6 +258,8 @@ static void reset_rate_I()
|
||||
g.pi_rate_pitch.reset_I();
|
||||
g.pi_acro_roll.reset_I();
|
||||
g.pi_acro_pitch.reset_I();
|
||||
g.pi_optflow_roll.reset_I();
|
||||
g.pi_optflow_pitch.reset_I();
|
||||
}
|
||||
|
||||
|
||||
@ -429,4 +431,70 @@ static int get_z_damping()
|
||||
static void init_z_damper()
|
||||
{
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// calculate modified roll/pitch depending upon optical flow values
|
||||
static int32_t
|
||||
get_of_roll(int32_t control_roll)
|
||||
{
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
//static int32_t of_roll = 0; // we use global variable to make logging easier
|
||||
static unsigned long last_of_roll_update = 0;
|
||||
static float prev_value = 0;
|
||||
float x_cm;
|
||||
|
||||
// check if new optflow data available
|
||||
if( optflow.last_update != last_of_roll_update) {
|
||||
last_of_roll_update = optflow.last_update;
|
||||
|
||||
// filter movement
|
||||
x_cm = (optflow.x_cm + prev_value) / 2.0 * 50.0;
|
||||
|
||||
// only stop roll if caller isn't modifying roll
|
||||
if( control_roll == 0 && current_loc.alt < 1500) {
|
||||
of_roll = g.pi_optflow_roll.get_pi(-x_cm, 1.0); // we could use the last update time to calculate the time change
|
||||
}else{
|
||||
g.pi_optflow_roll.reset_I();
|
||||
prev_value = 0;
|
||||
}
|
||||
}
|
||||
// limit maximum angle
|
||||
of_roll = constrain(of_roll, -1000, 1000);
|
||||
|
||||
return control_roll+of_roll;
|
||||
#else
|
||||
return control_roll;
|
||||
#endif
|
||||
}
|
||||
|
||||
static int32_t
|
||||
get_of_pitch(int32_t control_pitch)
|
||||
{
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
//static int32_t of_pitch = 0; // we use global variable to make logging easier
|
||||
static unsigned long last_of_pitch_update = 0;
|
||||
static float prev_value = 0;
|
||||
float y_cm;
|
||||
|
||||
// check if new optflow data available
|
||||
if( optflow.last_update != last_of_pitch_update ) {
|
||||
last_of_pitch_update = optflow.last_update;
|
||||
|
||||
// filter movement
|
||||
y_cm = (optflow.y_cm + prev_value) / 2.0 * 50.0;
|
||||
|
||||
// only stop roll if caller isn't modifying roll
|
||||
if( control_pitch == 0 && current_loc.alt < 1500 ) {
|
||||
of_pitch = g.pi_optflow_pitch.get_pi(y_cm, 1.0); // we could use the last update time to calculate the time change
|
||||
}else{
|
||||
g.pi_optflow_pitch.reset_I();
|
||||
prev_value = 0;
|
||||
}
|
||||
}
|
||||
// limit maximum angle
|
||||
of_pitch = constrain(of_pitch, -1000, 1000);
|
||||
return control_pitch+of_pitch;
|
||||
#else
|
||||
return control_pitch;
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -482,8 +482,12 @@ static void Log_Write_Optflow()
|
||||
DataFlash.WriteInt((int)optflow.dx);
|
||||
DataFlash.WriteInt((int)optflow.dy);
|
||||
DataFlash.WriteInt((int)optflow.surface_quality);
|
||||
DataFlash.WriteInt((int)optflow.x_cm);
|
||||
DataFlash.WriteInt((int)optflow.y_cm);
|
||||
DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
|
||||
DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
|
||||
DataFlash.WriteLong(of_roll);
|
||||
DataFlash.WriteLong(of_pitch);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
#endif
|
||||
}
|
||||
@ -495,15 +499,23 @@ static void Log_Read_Optflow()
|
||||
int16_t temp1 = DataFlash.ReadInt(); // 1
|
||||
int16_t temp2 = DataFlash.ReadInt(); // 2
|
||||
int16_t temp3 = DataFlash.ReadInt(); // 3
|
||||
float temp4 = DataFlash.ReadLong(); // 4
|
||||
float temp5 = DataFlash.ReadLong(); // 5
|
||||
int16_t temp4 = DataFlash.ReadInt(); // 4
|
||||
int16_t temp5 = DataFlash.ReadInt(); // 5
|
||||
float temp6 = DataFlash.ReadLong(); // 6
|
||||
float temp7 = DataFlash.ReadLong(); // 7
|
||||
int32_t temp8 = DataFlash.ReadLong(); // 8
|
||||
int32_t temp9 = DataFlash.ReadLong(); // 9
|
||||
|
||||
Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
|
||||
Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %d, %d\n"),
|
||||
temp1,
|
||||
temp2,
|
||||
temp3,
|
||||
temp4,
|
||||
temp5);
|
||||
temp5,
|
||||
temp6,
|
||||
temp7,
|
||||
temp8,
|
||||
temp9);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -178,6 +178,8 @@ public:
|
||||
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
|
||||
|
||||
|
||||
// 254,255: reserved
|
||||
@ -298,6 +300,9 @@ public:
|
||||
APM_PI pi_acro_roll;
|
||||
APM_PI pi_acro_pitch;
|
||||
|
||||
APM_PI pi_optflow_roll;
|
||||
APM_PI pi_optflow_pitch;
|
||||
|
||||
uint8_t junk;
|
||||
|
||||
// Note: keep initializers here in the same order as they are declared above.
|
||||
@ -417,6 +422,9 @@ public:
|
||||
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_IMAX * 100),
|
||||
pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100),
|
||||
|
||||
junk(0) // XXX just so that we can add things without worrying about the trailing comma
|
||||
{
|
||||
}
|
||||
|
||||
@ -348,7 +348,7 @@ static bool verify_land()
|
||||
static int16_t velocity_land = -1;
|
||||
|
||||
// land at .62 meter per second
|
||||
next_WP.alt = original_alt - ((millis() - land_start) / 16); // condition_value = our initial
|
||||
next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial
|
||||
|
||||
if (old_alt == 0)
|
||||
old_alt = current_loc.alt;
|
||||
@ -357,15 +357,17 @@ static bool verify_land()
|
||||
velocity_land = 2000;
|
||||
|
||||
|
||||
if ((current_loc.alt - home.alt) < 500){
|
||||
if ((current_loc.alt - home.alt) < 300){
|
||||
// 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) + (old_alt - current_loc.alt)) / 8;
|
||||
}
|
||||
|
||||
// remenber altitude for climb_rate
|
||||
old_alt = current_loc.alt;
|
||||
|
||||
if ((current_loc.alt - home.alt) < 300){
|
||||
|
||||
if ((current_loc.alt - home.alt) < 200){
|
||||
// don't bank to hold position
|
||||
wp_control = NO_NAV_MODE;
|
||||
|
||||
// Update by JLN for a safe AUTO landing
|
||||
@ -375,21 +377,14 @@ static bool verify_land()
|
||||
g.pi_throttle.reset_I();
|
||||
}
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// decide which sensor we're using
|
||||
if(sonar_alt < 40){
|
||||
land_complete = true;
|
||||
//Serial.println("Y");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
if((current_loc.alt - home.alt) < 300 && velocity_land <= 100){
|
||||
if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){
|
||||
land_complete = true;
|
||||
// reset manual_boost hack
|
||||
manual_boost = 0;
|
||||
|
||||
// reset old_alt
|
||||
old_alt = 0;
|
||||
init_disarm_motors();
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
//Serial.printf("N, %d\n", velocity_land);
|
||||
//Serial.printf("N_alt, %ld\n", next_WP.alt);
|
||||
|
||||
@ -58,7 +58,16 @@ static void update_commands()
|
||||
command_nav_queue.id = NO_COMMAND;
|
||||
}
|
||||
}else{
|
||||
// we are out of commands
|
||||
g.command_index = command_nav_index = 255;
|
||||
// if we are on the ground, enter stabilize, else Land
|
||||
if (land_complete == true){
|
||||
set_mode(STABILIZE);
|
||||
// disarm motors
|
||||
//init_disarm_motors();
|
||||
} else {
|
||||
set_mode(LAND);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -65,10 +65,10 @@
|
||||
// FRAME_CONFIG
|
||||
//
|
||||
#ifndef FRAME_CONFIG
|
||||
# define FRAME_CONFIG QUAD_FRAME
|
||||
# define FRAME_CONFIG QUAD_FRAME
|
||||
#endif
|
||||
#ifndef FRAME_ORIENTATION
|
||||
# define FRAME_ORIENTATION PLUS_FRAME
|
||||
# define FRAME_ORIENTATION X_FRAME
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
@ -170,7 +170,7 @@
|
||||
# endif
|
||||
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
||||
# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
|
||||
# define CONFIG_SONAR_SOURCE_ANALOG_PIN AN4
|
||||
# define CONFIG_SONAR_SOURCE_ANALOG_PIN A1
|
||||
# endif
|
||||
#else
|
||||
# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar
|
||||
@ -310,6 +310,23 @@
|
||||
#ifndef OPTFLOW_FOV
|
||||
# define OPTFLOW_FOV AP_OPTICALFLOW_ADNS3080_08_FOV
|
||||
#endif
|
||||
// optical flow based loiter PI values
|
||||
#ifndef OPTFLOW_ROLL_P
|
||||
#define OPTFLOW_ROLL_P 6.4
|
||||
#endif
|
||||
#ifndef OPTFLOW_ROLL_I
|
||||
#define OPTFLOW_ROLL_I 0.068
|
||||
#endif
|
||||
#ifndef OPTFLOW_PITCH_P
|
||||
#define OPTFLOW_PITCH_P 6.4
|
||||
#endif
|
||||
#ifndef OPTFLOW_PITCH_I
|
||||
#define OPTFLOW_PITCH_I 0.068
|
||||
#endif
|
||||
#ifndef OPTFLOW_IMAX
|
||||
#define OPTFLOW_IMAX 4
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// RADIO CONFIGURATION
|
||||
@ -468,6 +485,11 @@
|
||||
#endif
|
||||
|
||||
|
||||
// experimental feature for
|
||||
#ifndef WIND_COMP_STAB
|
||||
# define WIND_COMP_STAB 0
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
@ -18,6 +18,7 @@
|
||||
#define ROLL_PITCH_STABLE 0
|
||||
#define ROLL_PITCH_ACRO 1
|
||||
#define ROLL_PITCH_AUTO 2
|
||||
#define ROLL_PITCH_STABLE_OF 3
|
||||
|
||||
#define THROTTLE_MANUAL 0
|
||||
#define THROTTLE_HOLD 1
|
||||
@ -155,6 +156,10 @@
|
||||
#define CH6_Z_GAIN 15
|
||||
#define CH6_DAMP 16
|
||||
|
||||
// optical flow controller
|
||||
#define CH6_OPTFLOW_KP 17
|
||||
#define CH6_OPTFLOW_KI 18
|
||||
|
||||
|
||||
// nav byte mask
|
||||
// -------------
|
||||
@ -283,8 +288,8 @@ enum gcs_severity {
|
||||
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
|
||||
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*VOLT_DIV_RATIO
|
||||
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
|
||||
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1023.0))*VOLT_DIV_RATIO
|
||||
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1023.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
|
||||
//#define BARO_FILTER_SIZE 8
|
||||
|
||||
/* ************************************************************** */
|
||||
|
||||
@ -195,7 +195,7 @@ static void calc_loiter_pitch_roll()
|
||||
nav_pitch = -nav_pitch;
|
||||
}
|
||||
|
||||
// what's the update rate? 10hz GPS?
|
||||
#if WIND_COMP_STAB == 1
|
||||
static void calc_wind_compensation()
|
||||
{
|
||||
// this idea is a function that converts user input into I term for position hold
|
||||
@ -216,8 +216,25 @@ static void calc_wind_compensation()
|
||||
|
||||
// filter the input and apply it to out integrator value
|
||||
// nav_lon and nav_lat will be applied to normal flight
|
||||
nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16;
|
||||
nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16;
|
||||
|
||||
// This filter is far too fast!!!
|
||||
//nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16;
|
||||
//nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16;
|
||||
|
||||
nav_lon = g.pi_loiter_lon.get_integrator();
|
||||
nav_lat = g.pi_loiter_lat.get_integrator();
|
||||
|
||||
// Maybe a slower filter would work?
|
||||
if(g.pi_loiter_lon.get_integrator() > roll_tmp){
|
||||
g.pi_loiter_lon.set_integrator(nav_lon - 5);
|
||||
}else if(g.pi_loiter_lon.get_integrator() < roll_tmp){
|
||||
g.pi_loiter_lon.set_integrator(nav_lon + 5);
|
||||
}
|
||||
if(g.pi_loiter_lat.get_integrator() > pitch_tmp){
|
||||
g.pi_loiter_lat.set_integrator(nav_lat - 5);
|
||||
}else if(g.pi_loiter_lat.get_integrator() < pitch_tmp){
|
||||
g.pi_loiter_lat.set_integrator(nav_lat + 5);
|
||||
}
|
||||
|
||||
// save smoothed input to integrator
|
||||
g.pi_loiter_lon.set_integrator(nav_lon); // X
|
||||
@ -243,16 +260,15 @@ static void reduce_wind_compensation()
|
||||
tmp *= .98;
|
||||
g.pi_loiter_lat.set_integrator(tmp); // Y
|
||||
|
||||
#if 0
|
||||
// debug
|
||||
int16_t t1 = g.pi_loiter_lon.get_integrator();
|
||||
int16_t t2 = g.pi_loiter_lon.get_integrator();
|
||||
//int16_t t1 = g.pi_loiter_lon.get_integrator();
|
||||
//int16_t t2 = g.pi_loiter_lon.get_integrator();
|
||||
|
||||
//Serial.printf("reduce wind iterm X:%d Y:%d \n",
|
||||
// t1,
|
||||
// t2);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static int16_t calc_desired_speed(int16_t max_speed)
|
||||
{
|
||||
|
||||
@ -380,6 +380,8 @@ static void reset_I_all(void)
|
||||
g.pi_throttle.reset_I();
|
||||
g.pi_acro_roll.reset_I();
|
||||
g.pi_acro_pitch.reset_I();
|
||||
g.pi_optflow_roll.reset_I();
|
||||
g.pi_optflow_pitch.reset_I();
|
||||
}
|
||||
|
||||
|
||||
@ -426,10 +428,11 @@ static void startup_ground(void)
|
||||
|
||||
static void set_mode(byte mode)
|
||||
{
|
||||
if(control_mode == mode){
|
||||
//if(control_mode == mode){
|
||||
// don't switch modes if we are already in the correct mode.
|
||||
return;
|
||||
}
|
||||
// Serial.print("Double mode\n");
|
||||
//return;
|
||||
//}
|
||||
|
||||
// if we don't have GPS lock
|
||||
if(home_is_set == false){
|
||||
@ -458,6 +461,9 @@ static void set_mode(byte mode)
|
||||
// do not auto_land if we are leaving RTL
|
||||
auto_land_timer = 0;
|
||||
|
||||
// if we change modes, we must clear landed flag
|
||||
land_complete = false;
|
||||
|
||||
// debug to Serial terminal
|
||||
Serial.println(flight_mode_strings[control_mode]);
|
||||
|
||||
@ -531,12 +537,10 @@ static void set_mode(byte mode)
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
land_complete = false;
|
||||
yaw_mode = YAW_HOLD;
|
||||
roll_pitch_mode = ROLL_PITCH_AUTO;
|
||||
yaw_mode = LOITER_YAW;
|
||||
roll_pitch_mode = LOITER_RP;
|
||||
throttle_mode = THROTTLE_AUTO;
|
||||
next_WP = current_loc;
|
||||
next_WP.alt = 0;
|
||||
do_land();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
|
||||
@ -6,7 +6,9 @@
|
||||
// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
|
||||
//#define SERIAL3_BAUD 38400
|
||||
|
||||
# define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
// # define APM2_BETA_HARDWARE
|
||||
|
||||
|
||||
// You may also put an include statement here to point at another configuration file. This is convenient if you maintain
|
||||
// different configuration files for different aircraft or HIL simulation. See the examples below
|
||||
|
||||
@ -413,8 +413,9 @@ static void do_wait_delay()
|
||||
|
||||
static void do_change_alt()
|
||||
{
|
||||
condition_rate = next_nonnav_command.lat;
|
||||
condition_rate = abs((int)next_nonnav_command.lat);
|
||||
condition_value = next_nonnav_command.alt;
|
||||
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
|
||||
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
|
||||
next_WP.alt = condition_value; // For future nav calculations
|
||||
offset_altitude = 0; // For future nav calculations
|
||||
|
||||
@ -50,7 +50,7 @@ def takeoff(mavproxy, mav, alt_min = 30):
|
||||
'''takeoff get to 30m altitude'''
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
mavproxy.send('rc 3 1510\n')
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
if (m.alt < alt_min):
|
||||
wait_altitude(mav, alt_min, (alt_min + 5))
|
||||
@ -208,7 +208,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120):
|
||||
'''fly Simple, flying N then E'''
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1430\n')
|
||||
mavproxy.send('rc 3 1450\n')
|
||||
tstart = time.time()
|
||||
failed = False
|
||||
|
||||
@ -374,8 +374,9 @@ def fly_ArduCopter(viewerip=None):
|
||||
os.unlink(buildlog)
|
||||
os.link(logfile, buildlog)
|
||||
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
mavproxy.expect("Ready to FLY")
|
||||
# the received parameters can come before or after the ready to fly message
|
||||
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
||||
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
||||
|
||||
util.expect_setup_callback(mavproxy, expect_callback)
|
||||
|
||||
|
||||
@ -11,11 +11,11 @@ body {
|
||||
margin:0px;
|
||||
padding:0px;
|
||||
background-color: #fff;
|
||||
background-image: url(images/bg.png);
|
||||
background-image: url(/images/bg.png);
|
||||
}
|
||||
|
||||
#logo {
|
||||
background-image:url(images/logo.png);
|
||||
background-image:url(/images/logo.png);
|
||||
background-repeat:no-repeat;
|
||||
height: 120px;
|
||||
width: 420px;
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
<title>ArduPilot Automatic Testing</title>
|
||||
|
||||
<!--CSS -->
|
||||
<link href="css/main.css" rel="stylesheet" type="text/css" />
|
||||
<link href="/css/main.css" rel="stylesheet" type="text/css" />
|
||||
</head>
|
||||
|
||||
<body>
|
||||
|
||||
@ -10,9 +10,14 @@
|
||||
#define HEAD_BYTE1 0xA3
|
||||
#define HEAD_BYTE2 0x95
|
||||
|
||||
// NOTE: You must uncomment one of the following two lines
|
||||
DataFlash_APM2 DataFlash; // Uncomment this line if using APM2 hardware
|
||||
//DataFlash_APM1 DataFlash; // Uncomment this line if using APM1 hardware
|
||||
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.begin(115200);
|
||||
DataFlash.Init(); // DataFlash initialization
|
||||
|
||||
Serial.println("Dataflash Log Test 1.0");
|
||||
@ -24,14 +29,14 @@ void setup()
|
||||
Serial.print("Manufacturer:");
|
||||
Serial.print(int(DataFlash.df_manufacturer));
|
||||
Serial.print(",");
|
||||
Serial.print(int(DataFlash.df_device_0));
|
||||
Serial.print(",");
|
||||
Serial.print(int(DataFlash.df_device_1));
|
||||
Serial.print(DataFlash.df_device);
|
||||
Serial.println();
|
||||
|
||||
// We start to write some info (sequentialy) starting from page 1
|
||||
// This is similar to what we will do...
|
||||
DataFlash.StartWrite(1);
|
||||
Serial.println("After testing perform erase before using DataFlash for logging!");
|
||||
Serial.println("");
|
||||
Serial.println("Writing to flash... wait...");
|
||||
for (int i = 0; i < 1000; i++){ // Write 1000 packets...
|
||||
// We write packets of binary data... (without worry about nothing more)
|
||||
@ -96,5 +101,8 @@ void loop()
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
delay(10000);
|
||||
Serial.println("");
|
||||
Serial.println("Test complete. Test will repeat in 20 seconds");
|
||||
Serial.println("");
|
||||
delay(20000);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user