Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 10:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

2.0.40

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@3252 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-09-05 00:15:36 +00:00
parent 3ecf7b503c
commit 3ad21cc236
22 changed files with 815 additions and 1015 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduCopterMega/APM_Config.h
View File

@ -3,14 +3,12 @@
// GPS is auto-selected // GPS is auto-selected
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define HIL_MODE HIL_MODE_ATTITUDE //#define HIL_MODE HIL_MODE_ATTITUDE
//#define LOITER_TEST 1
//#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)
//#define FRAME_CONFIG QUAD_FRAME #define FRAME_CONFIG QUAD_FRAME
/* /*
options: options:
QUAD_FRAME QUAD_FRAME
@ -21,7 +19,7 @@
HELI_FRAME HELI_FRAME
*/ */
//#define FRAME_ORIENTATION X_FRAME #define FRAME_ORIENTATION X_FRAME
/* /*
PLUS_FRAME PLUS_FRAME
X_FRAME X_FRAME
@ -29,7 +27,7 @@
*/ */
//#define CHANNEL_6_TUNING CH6_NONE #define CHANNEL_6_TUNING CH6_NONE
/* /*
CH6_NONE CH6_NONE
CH6_STABILIZE_KP CH6_STABILIZE_KP
@ -45,9 +43,14 @@
CH6_TOP_BOTTOM_RATIO CH6_TOP_BOTTOM_RATIO
CH6_PMAX CH6_PMAX
CH6_RELAY CH6_RELAY
CH6_TRAVERSE_SPEED
*/ */
// experimental!! // See the config.h and defines.h files for how to set this up!
// Yaw is controled by targeting home. you will not have Yaw override. //
// flying too close to home may induce spins. // lets use SIMPLE mode for Roll and Pitch during Alt Hold
//#define SIMPLE_LOOK_AT_HOME 0 #define ALT_HOLD_RP ROLL_PITCH_SIMPLE
// lets use Manual throttle during Loiter
//#define LOITER_THR THROTTLE_MANUAL
# define RTL_YAW YAW_HOLD

581
ArduCopterMega/ArduCopterMega.pde
View File

@ -58,7 +58,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library #include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library #include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library #include <APM_PI.h> // PI library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library #include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library #include <AP_OpticalFlow.h> // Optical Flow library
@ -261,6 +261,13 @@ static const char* flight_mode_strings[] = {
//Vector3f accels_rot; //Vector3f accels_rot;
//float accel_gain = 20; //float accel_gain = 20;
// temp
int y_actual_speed;
int y_rate_error;
// calc the
int x_actual_speed;
int x_rate_error;
// Radio // Radio
// ----- // -----
@ -289,8 +296,6 @@ static boolean motor_auto_armed; // if true,
//int max_stabilize_dampener; // //int max_stabilize_dampener; //
//int max_yaw_dampener; // //int max_yaw_dampener; //
static boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold static boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
static byte yaw_debug;
static bool did_clear_yaw_control;
static Vector3f omega; static Vector3f omega;
// LED output // LED output
@ -314,6 +319,8 @@ static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static bool xtrack_enabled = false;
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
@ -336,6 +343,7 @@ static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static bool simple_bearing_is_set = false; static bool simple_bearing_is_set = false;
static long initial_simple_bearing; // used for Simple mode static long initial_simple_bearing; // used for Simple mode
// Acro // Acro
#if CH7_OPTION == DO_FLIP #if CH7_OPTION == DO_FLIP
static bool do_flip = false; static bool do_flip = false;
@ -349,11 +357,10 @@ static int airspeed; // m/s * 100
// --------------- // ---------------
static long bearing_error; // deg * 100 : 0 to 36000 static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude static long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline static long old_altitude;
static long distance_error; // distance to the WP static long distance_error; // distance to the WP
static long yaw_error; // how off are we pointed static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging static long long_error, lat_error; // temp for debugging
static int loiter_error_max;
// Battery Sensors // Battery Sensors
// --------------- // ---------------
@ -367,16 +374,11 @@ static float current_amps;
static float current_total; static float current_total;
static bool low_batt = false; static bool low_batt = false;
// Airspeed Sensors
// ----------------
// Barometer Sensor variables // Barometer Sensor variables
// -------------------------- // --------------------------
static long abs_pressure; static long abs_pressure;
static long ground_pressure; static long ground_pressure;
static int ground_temperature; static int ground_temperature;
static int32_t baro_filter[BARO_FILTER_SIZE];
static byte baro_filter_index;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
@ -384,18 +386,20 @@ static int sonar_alt;
static int baro_alt; static int baro_alt;
static int baro_alt_offset; static int baro_alt_offset;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate;
// flight mode specific // flight mode specific
// -------------------- // --------------------
static byte yaw_mode;
static byte roll_pitch_mode;
static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls static boolean takeoff_complete; // Flag for using take-off controls
static boolean land_complete; static boolean land_complete;
//static int takeoff_altitude;
static int landing_distance; // meters; static int landing_distance; // meters;
static long old_alt; // used for managing altitude rates static long old_alt; // used for managing altitude rates
static int velocity_land; static int velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
//static int throttle_slew; // used to smooth throttle tranistions
// Loiter management // Loiter management
// ----------------- // -----------------
@ -408,11 +412,12 @@ static unsigned long loiter_time_max; // millis : how long to stay in LOITER
static long nav_roll; // deg * 100 : target roll angle static long nav_roll; // deg * 100 : target roll angle
static long nav_pitch; // deg * 100 : target pitch angle static long nav_pitch; // deg * 100 : target pitch angle
static long nav_yaw; // deg * 100 : target yaw angle static long nav_yaw; // deg * 100 : target yaw angle
static long auto_yaw; // deg * 100 : target yaw angle
static long nav_lat; // for error calcs static long nav_lat; // for error calcs
static long nav_lon; // for error calcs static long nav_lon; // for error calcs
static int nav_throttle; // 0-1000 for throttle control static int nav_throttle; // 0-1000 for throttle control
static long throttle_integrator; // used to control when we calculate nav_throttle static long throttle_integrator; // used to integrate throttle output to predict battery life
static bool invalid_throttle; // used to control when we calculate nav_throttle static bool invalid_throttle; // used to control when we calculate nav_throttle
static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -478,24 +483,23 @@ static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm
// Performance monitoring // Performance monitoring
// ---------------------- // ----------------------
static long perf_mon_timer; static long perf_mon_timer;
static float imu_health; // Metric based on accel gain deweighting static float imu_health; // Metric based on accel gain deweighting
static int G_Dt_max; // Max main loop cycle time in milliseconds static int G_Dt_max; // Max main loop cycle time in milliseconds
static int gps_fix_count; static int gps_fix_count;
static byte gcs_messages_sent; static byte gcs_messages_sent;
// GCS // GCS
// --- // ---
static char GCS_buffer[53]; static char GCS_buffer[53];
static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
// System Timers // System Timers
// -------------- // --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count; static int mainLoop_count;
static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
@ -505,8 +509,9 @@ static unsigned long fiftyhz_loopTimer;
static uint8_t delta_ms_fiftyhz; static uint8_t delta_ms_fiftyhz;
static byte slow_loopCounter; static byte slow_loopCounter;
static int superslow_loopCounter; static int superslow_loopCounter;
static byte flight_timer; // for limiting the execution of flight mode thingys static byte alt_timer; // for limiting the execution of flight mode thingys
static byte simple_timer; // for limiting the execution of flight mode thingys
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
@ -514,9 +519,9 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS
static unsigned long elapsedTime; // for doing custom events static unsigned long elapsedTime; // for doing custom events
static float load; // % MCU cycles used static float load; // % MCU cycles used
static byte counter_one_herz; static byte counter_one_herz;
static bool GPS_enabled = false; static bool GPS_enabled = false;
static byte loop_step; static byte loop_step;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Top-level logic // Top-level logic
@ -610,7 +615,9 @@ static void fast_loop()
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
// --------------------------------------- // ---------------------------------------
update_current_flight_mode(); update_yaw_mode();
update_roll_pitch_mode();
update_throttle_mode();
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
@ -694,13 +701,13 @@ static void medium_loop()
// Read altitude from sensors // Read altitude from sensors
// -------------------------- // --------------------------
update_alt(); update_altitude();
// altitude smoothing // altitude smoothing
// ------------------ // ------------------
//calc_altitude_smoothing_error(); //calc_altitude_smoothing_error();
calc_altitude_error(); altitude_error = get_altitude_error();
// invalidate the throttle hold value // invalidate the throttle hold value
// ---------------------------------- // ----------------------------------
@ -1091,8 +1098,42 @@ void update_location(void)
} }
#endif #endif
void update_yaw_mode(void)
{
switch(yaw_mode){
case YAW_ACRO:
g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in);
return;
break;
void update_current_flight_mode(void) case YAW_HOLD:
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
//Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw);
break;
case YAW_LOOK_AT_HOME:
// copter will always point at home
if(home_is_set){
nav_yaw = point_at_home_yaw();
} else {
nav_yaw = 0;
}
break;
case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20);
nav_yaw = wrap_360(nav_yaw);
break;
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
//Serial.printf("4: %d\n",g.rc_4.servo_out);
}
void update_roll_pitch_mode(void)
{ {
#if CH7_OPTION == DO_FLIP #if CH7_OPTION == DO_FLIP
if (do_flip){ if (do_flip){
@ -1101,221 +1142,87 @@ void update_current_flight_mode(void)
} }
#endif #endif
if(control_mode == AUTO){ int control_roll, control_pitch;
// this is a hack to prevent run up of the throttle I term for alt hold
if(command_must_ID == MAV_CMD_NAV_TAKEOFF){
invalid_throttle = (g.rc_3.control_in != 0);
// make invalid_throttle false if we are waiting to take off.
}
switch(command_must_ID){ switch(roll_pitch_mode){
default: case ROLL_PITCH_ACRO:
// mix in user control with Nav control // Roll control
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll); g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
// Roll control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out);
// Throttle control
if(invalid_throttle){
auto_throttle();
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, .5);
break;
}
}else{
switch(control_mode){
case ACRO:
// Roll control
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
// Pitch control
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// Yaw control
g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in);
break;
case STABILIZE:
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
// Roll control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
//Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out);
break;
case SIMPLE:
flight_timer++;
if(flight_timer > 6){
flight_timer = 0;
// make sure this is always 0
simple_WP.lat = 0;
simple_WP.lng = 0;
next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres
next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres
// calc a new bearing
nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing;
nav_bearing = wrap_360(nav_bearing);
wp_distance = get_distance(&simple_WP, &next_WP);
calc_bearing_error();
/*
Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
next_WP.lat,
next_WP.lng,
nav_bearing,
wp_distance,
initial_simple_bearing,
bearing_error);
*/
// get nav_pitch and nav_roll
calc_simple_nav();
calc_nav_output();
}
/*
#if SIMPLE_LOOK_AT_HOME == 0
// This is typical yaw behavior
// are we at rest? reset nav_yaw
if(g.rc_3.control_in == 0){
clear_yaw_control();
}else{
// Yaw control
output_manual_yaw();
}
#else
// This is experimental,
// copter will always point at home
if(home_is_set)
point_at_home_yaw();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
#endif
*/
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
// Roll control
g.rc_1.servo_out = get_stabilize_roll(nav_roll);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(nav_pitch);
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
//Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew);
// Pitch control
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
return;
break; break;
case ALT_HOLD: case ROLL_PITCH_STABLE:
// allow interactive changing of atitude control_roll = g.rc_1.control_in;
adjust_altitude(); control_pitch = g.rc_2.control_in;
break;
// calcualte new nav_yaw offset case ROLL_PITCH_SIMPLE:
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle simple_timer++;
if(simple_timer > 5){
simple_timer = 0;
// Roll control int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
// Pitch control // pre-calc rotation (stored in real degrees)
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); // roll
float cos_x = sin(radians(90 - delta));
// pitch
float sin_y = cos(radians(90 - delta));
// Throttle control // Rotate input by the initial bearing
if(invalid_throttle){ // roll
auto_throttle(); nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y;
} // pitch
nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x);
}
control_roll = nav_roll;
control_pitch = nav_pitch;
break;
// Yaw control case ROLL_PITCH_AUTO:
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); // mix in user control with Nav control
break; control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
break;
}
case CIRCLE: // Roll control
case GUIDED: g.rc_1.servo_out = get_stabilize_roll(control_roll);
case RTL:
// mix in user control with Nav control
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
// Roll control // Pitch control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out); g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
}
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out);
// Throttle control void update_throttle_mode(void)
if(invalid_throttle){ {
auto_throttle(); switch(throttle_mode){
} case THROTTLE_MANUAL:
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
break;
// Yaw control case THROTTLE_HOLD:
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.25); // allow interactive changing of atitude
break; adjust_altitude();
case LOITER: // fall through
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle
// allow interactive changing of atitude case THROTTLE_AUTO:
adjust_altitude(); if(invalid_throttle){
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 150); //150 = target speed of 1.5m/s
// mix in user control with Nav control // apply throttle control
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll); g.rc_3.servo_out = get_throttle(nav_throttle);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
// Roll control // clear the new data flag
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out); invalid_throttle = false;
}
// Pitch control break;
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out);
// Throttle control
if(invalid_throttle){
auto_throttle();
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
break;
default:
//Serial.print("$");
break;
}
} }
} }
@ -1331,7 +1238,7 @@ static void update_navigation()
// note: wp_control is handled by commands_logic // note: wp_control is handled by commands_logic
// calculates desired Yaw // calculates desired Yaw
update_nav_yaw(); update_auto_yaw();
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
@ -1339,40 +1246,26 @@ static void update_navigation()
case GUIDED: case GUIDED:
case RTL: case RTL:
if(wp_distance > 5){ if(wp_distance > 4){
// calculates desired Yaw // calculates desired Yaw
// XXX this is an experiment // XXX this is an experiment
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
update_nav_yaw(); update_auto_yaw();
#endif #endif
wp_control = WP_MODE;
}else{
set_mode(LOITER);
xtrack_enabled = false;
} }
#if LOITER_TEST == 0
// calc a rate dampened pitch to the target
calc_rate_nav(g.waypoint_speed_max.get());
// rotate that pitch to the copter frame of reference
calc_nav_output();
#else
// rate based test
// calc error to target
calc_loiter_nav2();
// use error as a rate towards the target
calc_rate_nav2(long_error/2, lat_error/2);
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
#endif
// calculates the desired Roll and Pitch
update_nav_wp();
break; break;
// switch passthrough to LOITER // switch passthrough to LOITER
case LOITER: case LOITER:
// are we Traversing or Loitering?
//wp_control = (wp_distance < 20) ? LOITER_MODE : WP_MODE;
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
@ -1381,20 +1274,43 @@ static void update_navigation()
case CIRCLE: case CIRCLE:
yaw_tracking = MAV_ROI_WPNEXT; yaw_tracking = MAV_ROI_WPNEXT;
// calculates desired Yaw // calculates desired Yaw
update_nav_yaw(); update_auto_yaw();
// hack to elmininate crosstrack effect // calc the lat and long error to the target
crosstrack_bearing = target_bearing; calc_location_error();
// get a new nav_bearing // use error as the desired rate towards the target
update_loiter(); // nav_lon, nav_lat is calculated
calc_nav_rate(long_error, lat_error, 200, 0); // move at 3m/s, minimum 2m/s
// calc a rate dampened pitch to the target // rotate nav_lat and nav_long by nav_bearing so we circle the target
calc_rate_nav(400); {
// rotate the desired direction based on the distance to
// create a vector field
int angle = get_loiter_angle();
// rotate that pitch to the copter frame of reference // pre-calc rotation (stored in real degrees)
calc_nav_output(); // roll
float cos_x = cos(radians(90 - angle));
// pitch
float sin_y = sin(radians(90 - angle));
// Rotate input by the initial bearing
// roll
long temp_roll = nav_lon * sin_y - nav_lat * cos_x;
// pitch
long temp_pitch = nav_lon * cos_x + nav_lat * sin_y;
// we used temp values to not change the equations
// in mid-rotation
nav_lon = temp_roll;
nav_lat = temp_pitch;
}
// rotate pitch and roll to the copter frame of reference
calc_nav_pitch_roll();
break; break;
} }
@ -1430,12 +1346,19 @@ static void update_trig(void){
cos_roll_x = temp.c.z / cos_pitch_x; cos_roll_x = temp.c.z / cos_pitch_x;
sin_roll_y = temp.c.y / cos_pitch_x; sin_roll_y = temp.c.y / cos_pitch_x;
//flat:
// 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00,
// 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00,
// 180 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy: 0.00, sy: -1.00,
// 270 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy: -1.00, sy: 0.00,
//Vector3f accel_filt = imu.get_accel_filtered(); //Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); //accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
} }
// updated at 10hz // updated at 10hz
static void update_alt() static void update_altitude()
{ {
altitude_sensor = BARO; altitude_sensor = BARO;
@ -1452,6 +1375,14 @@ static void update_alt()
baro_alt = read_barometer(); baro_alt = read_barometer();
if(baro_alt < 1000){ if(baro_alt < 1000){
#if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar
float temp = cos_pitch_x * cos_roll_x;
temp = max(temp, 0.707);
sonar_alt = (float)sonar_alt * temp;
#endif
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
@ -1465,26 +1396,26 @@ static void update_alt()
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
} }
altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer
old_altitude = current_loc.alt;
#endif #endif
} }
static void static void
adjust_altitude() adjust_altitude()
{ {
flight_timer++; alt_timer++;
if(flight_timer >= 2){ if(alt_timer >= 2){
flight_timer = 0; alt_timer = 0;
if(g.rc_3.control_in <= 200){ if(g.rc_3.control_in <= 200){
next_WP.alt -= 3; // 1 meter per second next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 900)); // don't go more than 4 meters below current location next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
}else if (g.rc_3.control_in > 700){ }else if (g.rc_3.control_in > 700){
next_WP.alt += 4; // 1 meter per second next_WP.alt += 1; // 1 meter per second
//next_WP.alt = min((current_loc.alt + 400), next_WP.alt); // don't go more than 4 meters below current location next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location
next_WP.alt = min(next_WP.alt, (current_loc.alt + 600)); // don't go more than 4 meters below current location
} }
next_WP.alt = max(next_WP.alt, 100); // don't go more than 4 meters below current location
} }
} }
@ -1492,47 +1423,51 @@ static void tuning(){
//Outer Loop : Attitude //Outer Loop : Attitude
#if CHANNEL_6_TUNING == CH6_STABILIZE_KP #if CHANNEL_6_TUNING == CH6_STABILIZE_KP
g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
g.pid_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_KP #elif CHANNEL_6_TUNING == CH6_YAW_KP
g.pid_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 g.pi_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_YAW_KI #elif CHANNEL_6_TUNING == CH6_YAW_KI
g.pid_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); g.pi_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0);
//Inner Loop : Rate //Inner Loop : Rate
#elif CHANNEL_6_TUNING == CH6_RATE_KP #elif CHANNEL_6_TUNING == CH6_RATE_KP
g.pid_rate_roll.kP((float)g.rc_6.control_in / 1000.0); g.pi_rate_roll.kP((float)g.rc_6.control_in / 1000.0);
g.pid_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); g.pi_rate_pitch.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_RATE_KI #elif CHANNEL_6_TUNING == CH6_RATE_KI
g.pid_rate_roll.kI((float)g.rc_6.control_in / 1000.0); g.pi_rate_roll.kI((float)g.rc_6.control_in / 1000.0);
g.pid_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); g.pi_rate_pitch.kI((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
g.pid_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); g.pi_rate_yaw.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI
g.pid_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); g.pi_rate_yaw.kI((float)g.rc_6.control_in / 1000.0);
//Altitude Hold //Altitude Hold
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
g.pid_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1 g.pi_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
g.pid_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 g.pi_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1
//Extras //Extras
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0; g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED
g.waypoint_speed_max = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_PMAX #elif CHANNEL_6_TUNING == CH6_PMAX
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
@ -1542,8 +1477,6 @@ static void tuning(){
if(g.rc_6.control_in >= 400) relay_off(); if(g.rc_6.control_in >= 400) relay_off();
#endif #endif
} }
static void update_nav_wp() static void update_nav_wp()
@ -1551,74 +1484,46 @@ static void update_nav_wp()
// XXX Guided mode!!! // XXX Guided mode!!!
if(wp_control == LOITER_MODE){ if(wp_control == LOITER_MODE){
#if LOITER_TEST == 0
// calc a pitch to the target // calc a pitch to the target
calc_loiter_nav(); calc_location_error();
// use error as the desired rate towards the target
calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
calc_loiter_output(); calc_nav_pitch_roll();
#else
// calc error to target
calc_loiter_nav2();
// use error as a rate towards the target
calc_rate_nav2(long_error/2, lat_error/2);
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
#endif
} else { } else {
// how far are we from the ideal trajectory? // for long journey's reset the wind resopnse
// this pushes us back on course // it assumes we are standing still.
update_crosstrack(); g.pi_loiter_lat.reset_I();
g.pi_loiter_lat.reset_I();
// calc a rate dampened pitch to the target // calc the lat and long error to the target
calc_rate_nav(g.waypoint_speed_max.get()); calc_location_error();
// rotate that pitch to the copter frame of reference // use error as the desired rate towards the target
calc_nav_output(); calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 100);
// rotate pitch and roll to the copter frame of reference
calc_nav_pitch_roll();
} }
} }
static void update_nav_yaw() static void update_auto_yaw()
{ {
// this tracks a location so the copter is always pointing towards it. // this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){ if(yaw_tracking == MAV_ROI_LOCATION){
nav_yaw = get_bearing(&current_loc, &target_WP); auto_yaw = get_bearing(&current_loc, &target_WP);
}else if(yaw_tracking == MAV_ROI_WPNEXT){ }else if(yaw_tracking == MAV_ROI_WPNEXT){
nav_yaw = target_bearing; auto_yaw = target_bearing;
} }
} }
static void point_at_home_yaw() static long point_at_home_yaw()
{ {
nav_yaw = get_bearing(&current_loc, &home); return get_bearing(&current_loc, &home);
}
static void auto_throttle()
{
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error);
// apply throttle control
//g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew);
g.rc_3.servo_out = get_throttle(nav_throttle);
//if(motor_armed){
// remember throttle offset
//throttle_slew = g.rc_3.servo_out - g.rc_3.control_in;
//}else{
// don't allow
//throttle_slew = 0;
//}
// clear the new data flag
invalid_throttle = false;
//Serial.printf("wp:%d, \te:%d \tt%d \t%d\n", (int)next_WP.alt, (int)altitude_error, nav_throttle, g.rc_3.servo_out);
} }

77
ArduCopterMega/Attitude.pde
View File

@ -13,13 +13,13 @@ get_stabilize_roll(long target_angle)
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// desired Rate: // desired Rate:
rate = g.pid_stabilize_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_stabilize_roll.get_pi(error, delta_ms_fast_loop);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.x) * 100.0); error = rate - (long)(degrees(omega.x) * 100.0);
rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif #endif
@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle)
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// desired Rate: // desired Rate:
rate = g.pid_stabilize_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_stabilize_pitch.get_pi(error, delta_ms_fast_loop);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.y) * 100.0); error = rate - (long)(degrees(omega.y) * 100.0);
rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif #endif
@ -54,32 +54,31 @@ get_stabilize_pitch(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
#define YAW_ERROR_MAX 2000
static int static int
get_stabilize_yaw(long target_angle, float scaler) get_stabilize_yaw(long target_angle)
{ {
long error; long error;
long rate; long rate;
error = wrap_180(target_angle - dcm.yaw_sensor); yaw_error = wrap_180(target_angle - dcm.yaw_sensor);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -4500, 4500); yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
rate = g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop);
// desired Rate:
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler);
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
if( ! g.heli_ext_gyro_enabled ) { if( ! g.heli_ext_gyro_enabled ) {
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.z) * 100.0); error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
} }
#else #else
// Rate P: // Rate P:
error = rate - (long)(degrees(omega.z) * 100.0); error = rate - (long)(degrees(omega.z) * 100.0);
rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
//Serial.printf("%d\t%d\n", (int)error, (int)rate); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif #endif
@ -87,6 +86,26 @@ get_stabilize_yaw(long target_angle, float scaler)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
#define ALT_ERROR_MAX 300
static int
get_nav_throttle(long z_error, int target_speed)
{
int rate_error;
int throttle;
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
target_speed = z_error * scaler;
rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -90, 90);
throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
return g.throttle_cruise + rate_error;
}
static int static int
get_rate_roll(long target_rate) get_rate_roll(long target_rate)
{ {
@ -94,7 +113,7 @@ get_rate_roll(long target_rate)
target_rate = constrain(target_rate, -2500, 2500); target_rate = constrain(target_rate, -2500, 2500);
error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
target_rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); target_rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop);
// output control: // output control:
return (int)constrain(target_rate, -2500, 2500); return (int)constrain(target_rate, -2500, 2500);
@ -107,7 +126,7 @@ get_rate_pitch(long target_rate)
target_rate = constrain(target_rate, -2500, 2500); target_rate = constrain(target_rate, -2500, 2500);
error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
target_rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); target_rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop);
// output control: // output control:
return (int)constrain(target_rate, -2500, 2500); return (int)constrain(target_rate, -2500, 2500);
@ -119,7 +138,7 @@ get_rate_yaw(long target_rate)
long error; long error;
error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
target_rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); target_rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop);
// output control: // output control:
return (int)constrain(target_rate, -2500, 2500); return (int)constrain(target_rate, -2500, 2500);
@ -128,15 +147,16 @@ get_rate_yaw(long target_rate)
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
static void static void reset_nav_I(void)
reset_nav_I(void)
{ {
g.pid_nav_lat.reset_I(); g.pi_loiter_lat.reset_I();
g.pid_nav_lon.reset_I(); g.pi_loiter_lat.reset_I();
g.pid_nav_wp.reset_I();
g.pid_crosstrack.reset_I(); g.pi_nav_lat.reset_I();
g.pid_throttle.reset_I(); g.pi_nav_lon.reset_I();
// I removed these, they don't seem to be needed.
g.pi_crosstrack.reset_I();
g.pi_throttle.reset_I();
} }
@ -146,8 +166,7 @@ throttle control
// user input: // user input:
// ----------- // -----------
static int static int get_throttle(int throttle_input)
get_throttle(int throttle_input)
{ {
throttle_input = (float)throttle_input * angle_boost(); throttle_input = (float)throttle_input * angle_boost();
//throttle_input = max(throttle_slew, throttle_input); //throttle_input = max(throttle_slew, throttle_input);
@ -177,6 +196,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
} }
} }
} }
/* /*
static int alt_hold_velocity() static int alt_hold_velocity()
{ {
@ -185,7 +205,8 @@ static int alt_hold_velocity()
error = min(error, 200); error = min(error, 200);
error = 1 - (error/ 200.0); error = 1 - (error/ 200.0);
return (accels_rot.z + 9.81) * accel_gain * error; return (accels_rot.z + 9.81) * accel_gain * error;
}*/ }
*/
static float angle_boost() static float angle_boost()
{ {

2
ArduCopterMega/GCS_Standard.pde
View File

@ -179,7 +179,7 @@ static void send_message(byte id, long param) {
case 0x21: templong = nav_bearing; break; case 0x21: templong = nav_bearing; break;
case 0x22: templong = bearing_error; break; case 0x22: templong = bearing_error; break;
case 0x23: templong = crosstrack_bearing; break; case 0x23: templong = crosstrack_bearing; break;
case 0x24: templong = crosstrack_error; break; case 0x24: templong = crosstrack_correction; break;
case 0x25: templong = altitude_error; break; case 0x25: templong = altitude_error; break;
case 0x26: templong = wp_radius; break; case 0x26: templong = wp_radius; break;
case 0x27: templong = loiter_radius; break; case 0x27: templong = loiter_radius; break;

53
ArduCopterMega/Log.pde
View File

@ -104,13 +104,9 @@ dump_log(uint8_t argc, const Menu::arg *argv)
// check that the requested log number can be read // check that the requested log number can be read
dump_log = argv[1].i; dump_log = argv[1].i;
if(dump_log == 99){ if (/*(argc != 2) || */ (dump_log < 1)) {
Log_Read(1, 4096);
}
if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log # %d\n"), dump_log); Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
Log_Read(1, 4095); Log_Read(0, 4095);
erase_logs(NULL, NULL); erase_logs(NULL, NULL);
return(-1); return(-1);
} }
@ -139,6 +135,14 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
for(int j = 1; j < 4096; j++) for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j); DataFlash.PageErase(j);
clear_header();
Serial.printf_P(PSTR("\nLog erased.\n"));
return (0);
}
static void clear_header()
{
DataFlash.StartWrite(1); DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -146,8 +150,6 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
DataFlash.WriteByte(0); DataFlash.WriteByte(0);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite(); DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n"));
return (0);
} }
static int8_t static int8_t
@ -537,11 +539,19 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)(target_bearing/100)); // 3 DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4 DataFlash.WriteInt((int)(nav_bearing/100)); // 4
DataFlash.WriteInt(y_actual_speed); // 4
DataFlash.WriteInt(y_rate_error); // 4
DataFlash.WriteInt(x_actual_speed); // 4
DataFlash.WriteInt(x_rate_error); // 4
DataFlash.WriteInt((int)long_error); // 5 DataFlash.WriteInt((int)long_error); // 5
DataFlash.WriteInt((int)lat_error); // 6 DataFlash.WriteInt((int)lat_error); // 6
DataFlash.WriteInt((int)nav_lon); // 7 DataFlash.WriteInt((int)nav_lon); // 7
DataFlash.WriteInt((int)nav_lat); // 8 DataFlash.WriteInt((int)nav_lat); // 8
DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7
DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -550,7 +560,9 @@ static void Log_Read_Nav_Tuning()
{ {
// 1 2 3 4 // 1 2 3 4
Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, " Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, "
"%d, %d, %d, %d\n"), "%d, %d, %d, %d, "
"%d, %d, %d, %d, "
"%d, %d\n"),
DataFlash.ReadInt(), //distance DataFlash.ReadInt(), //distance
DataFlash.ReadByte(), //bitmask DataFlash.ReadByte(), //bitmask
@ -559,6 +571,14 @@ static void Log_Read_Nav_Tuning()
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt()); //nav bearing DataFlash.ReadInt()); //nav bearing
} }
@ -576,11 +596,9 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw // yaw
DataFlash.WriteByte(yaw_debug);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error/100); DataFlash.WriteInt((int)yaw_error/100);
DataFlash.WriteInt((int)(omega.z * 100));
// Alt hold // Alt hold
DataFlash.WriteInt(g.rc_3.servo_out); DataFlash.WriteInt(g.rc_3.servo_out);
@ -589,7 +607,7 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)next_WP.alt); // DataFlash.WriteInt((int)next_WP.alt); //
DataFlash.WriteInt((int)altitude_error); // DataFlash.WriteInt((int)altitude_error); //
DataFlash.WriteInt((int)g.pid_throttle.get_integrator()); DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -598,9 +616,11 @@ static void Log_Write_Control_Tuning()
// Read an control tuning packet // Read an control tuning packet
static void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()
{ {
Serial.printf_P(PSTR( "CTUN, %d, %d, " Serial.printf_P(PSTR( "CTUN, "
"%d, %d, %d, %d, %1.4f, " "%d, %d, "
"%d, %d, %d, %d, %d, %d\n"), "%d, %d, %ld, "
"%d, %d, %ld, "
"%d, %d, %ld\n"),
// Control // Control
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -611,7 +631,8 @@ static void Log_Read_Control_Tuning()
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
DataFlash.ReadInt(), DataFlash.ReadInt(),
(float)DataFlash.ReadInt(),// Gyro Rate //(float)DataFlash.ReadInt(),// Gyro Rate
DataFlash.ReadLong(),
// Alt Hold // Alt Hold
DataFlash.ReadInt(), DataFlash.ReadInt(),

2
ArduCopterMega/Mavlink_Common.h
View File

@ -147,7 +147,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
wp_distance, wp_distance,
altitude_error / 1.0e2, altitude_error / 1.0e2,
0, 0,
crosstrack_error); 0);
//} //}
break; break;
} }

87
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly // The increment will prevent old parameters from being used incorrectly
// by newer code. // by newer code.
// //
static const uint16_t k_format_version = 106; static const uint16_t k_format_version = 107;
// The parameter software_type is set up solely for ground station use // The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega) // and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -151,23 +151,20 @@ public:
k_param_waypoint_speed_max, k_param_waypoint_speed_max,
// //
// 240: PID Controllers // 240: PI/D Controllers
// //
// Heading-to-roll PID: k_param_pi_rate_roll = 240,
// heading error from commnd to roll command deviation from trim k_param_pi_rate_pitch,
// (bank to turn strategy) k_param_pi_rate_yaw,
// k_param_pi_stabilize_roll,
k_param_pid_rate_roll = 240, k_param_pi_stabilize_pitch,
k_param_pid_rate_pitch, k_param_pi_stabilize_yaw,
k_param_pid_rate_yaw, k_param_pi_loiter_lat,
k_param_pid_stabilize_roll, k_param_pi_loiter_lon,
k_param_pid_stabilize_pitch, k_param_pi_nav_lat,
k_param_pid_stabilize_yaw, k_param_pi_nav_lon,
k_param_pid_nav_lat, k_param_pi_throttle,
k_param_pid_nav_lon, k_param_pi_crosstrack,
k_param_pid_nav_wp,
k_param_pid_throttle,
k_param_pid_crosstrack,
// 254,255: reserved // 254,255: reserved
@ -248,7 +245,7 @@ public:
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7) AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
#endif #endif
// RC channels // RC channels
RC_Channel rc_1; RC_Channel rc_1;
RC_Channel rc_2; RC_Channel rc_2;
RC_Channel rc_3; RC_Channel rc_3;
@ -260,18 +257,23 @@ public:
RC_Channel rc_camera_pitch; RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll; RC_Channel rc_camera_roll;
// PID controllers // PI/D controllers
PID pid_rate_roll; APM_PI pi_rate_roll;
PID pid_rate_pitch; APM_PI pi_rate_pitch;
PID pid_rate_yaw; APM_PI pi_rate_yaw;
PID pid_stabilize_roll;
PID pid_stabilize_pitch; APM_PI pi_stabilize_roll;
PID pid_stabilize_yaw; APM_PI pi_stabilize_pitch;
PID pid_nav_lat; APM_PI pi_stabilize_yaw;
PID pid_nav_lon;
PID pid_nav_wp; APM_PI pi_loiter_lat;
PID pid_throttle; APM_PI pi_loiter_lon;
PID pid_crosstrack;
APM_PI pi_nav_lat;
APM_PI pi_nav_lon;
APM_PI pi_throttle;
APM_PI pi_crosstrack;
uint8_t junk; uint8_t junk;
@ -355,23 +357,24 @@ public:
rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")), rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")),
rc_camera_roll (k_param_rc_10, PSTR("RC_CR_")), rc_camera_roll (k_param_rc_10, PSTR("RC_CR_")),
// PID controller group key name initial P initial I initial D initial imax // PI controller group key name initial P initial I initial imax
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, 0, RATE_ROLL_IMAX * 100), pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100),
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, 0, RATE_PITCH_IMAX * 100), pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100),
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, 0, RATE_YAW_IMAX * 100), pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100),
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100), pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100), pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, STABILIZE_YAW_IMAX * 100), pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), pi_loiter_lat (k_param_pi_loiter_lat, PSTR("LOITER_LAT_"),LOITER_P, LOITER_I, LOITER_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), pi_loiter_lon (k_param_pi_loiter_lon, PSTR("LOITER_LON_"),LOITER_P, LOITER_I, LOITER_IMAX * 100),
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
pid_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100),
pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, XTRACK_IMAX), pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_throttle (k_param_pi_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX),
pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX),
junk(0) // XXX just so that we can add things without worrying about the trailing comma junk(0) // XXX just so that we can add things without worrying about the trailing comma
{ {

1
ArduCopterMega/commands.pde
View File

@ -233,6 +233,7 @@ static void init_home()
// Save prev loc this makes the calcs look better before commands are loaded // Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home; prev_WP = home;
next_WP = home;
} }

5
ArduCopterMega/commands_logic.pde
View File

@ -5,9 +5,6 @@
/********************************************************************************/ /********************************************************************************/
static void handle_process_must() static void handle_process_must()
{ {
// clear nav_lat, this is how we pitch towards the target based on speed
nav_lat = 0;
switch(next_command.id){ switch(next_command.id){
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
@ -203,6 +200,7 @@ static void do_RTL(void)
temp.alt = read_alt_to_hold(); temp.alt = read_alt_to_hold();
//so we know where we are navigating from //so we know where we are navigating from
// --------------------------------------
next_WP = current_loc; next_WP = current_loc;
// Loads WP from Memory // Loads WP from Memory
@ -210,6 +208,7 @@ static void do_RTL(void)
set_next_WP(&temp); set_next_WP(&temp);
// output control mode to the ground station // output control mode to the ground station
// -----------------------------------------
gcs.send_message(MSG_HEARTBEAT); gcs.send_message(MSG_HEARTBEAT);
} }

3
ArduCopterMega/commands_process.pde
View File

@ -67,9 +67,6 @@ static void verify_commands(void)
if(verify_must()){ if(verify_must()){
//Serial.printf("verified must cmd %d\n" , command_must_index); //Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND; command_must_index = NO_COMMAND;
// reset rate controlled nav
g.pid_nav_wp.reset_I();
}else{ }else{
//Serial.printf("verified must false %d\n" , command_must_index); //Serial.printf("verified must false %d\n" , command_must_index);
} }

168
ArduCopterMega/config.h
View File

@ -300,34 +300,120 @@
//////////////////////////////////////////////////////////////////////////////
// Attitude Control
//
// SIMPLE Mode
#ifndef SIMPLE_YAW
# define SIMPLE_YAW YAW_HOLD
#endif
#ifndef SIMPLE_RP
# define SIMPLE_RP ROLL_PITCH_SIMPLE
#endif
#ifndef SIMPLE_THR
# define SIMPLE_THR THROTTLE_MANUAL
#endif
// Alt Hold Mode
#ifndef ALT_HOLD_YAW
# define ALT_HOLD_YAW YAW_HOLD
#endif
#ifndef ALT_HOLD_RP
# define ALT_HOLD_RP ROLL_PITCH_STABLE
#endif
#ifndef ALT_HOLD_THR
# define ALT_HOLD_THR THROTTLE_HOLD
#endif
// AUTO Mode
#ifndef AUTO_YAW
# define AUTO_YAW YAW_AUTO
#endif
#ifndef AUTO_RP
# define AUTO_RP ROLL_PITCH_AUTO
#endif
#ifndef AUTO_THR
# define AUTO_THR THROTTLE_AUTO
#endif
// CIRCLE Mode
#ifndef CIRCLE_YAW
# define CIRCLE_YAW YAW_HOLD
#endif
#ifndef CIRCLE_RP
# define CIRCLE_RP ROLL_PITCH_AUTO
#endif
#ifndef CIRCLE_THR
# define CIRCLE_THR THROTTLE_HOLD
#endif
// LOITER Mode
#ifndef LOITER_YAW
# define LOITER_YAW YAW_HOLD
#endif
#ifndef LOITER_RP
# define LOITER_RP ROLL_PITCH_AUTO
#endif
#ifndef LOITER_THR
# define LOITER_THR THROTTLE_HOLD
#endif
// RTL Mode
#ifndef RTL_YAW
# define RTL_YAW YAW_AUTO
#endif
#ifndef RTL_RP
# define RTL_RP ROLL_PITCH_AUTO
#endif
#ifndef RTL_THR
# define RTL_THR THROTTLE_AUTO
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Attitude Control // Attitude Control
// //
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_P 3.6
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025 # define STABILIZE_ROLL_I 0.02
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 10 // degrees # define STABILIZE_ROLL_IMAX 1.5 // degrees
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_P 3.6
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.025 # define STABILIZE_PITCH_I 0.02
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 10 // degrees # define STABILIZE_PITCH_IMAX 1.5 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Rate Control // Rate Control
// //
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P .12 # define RATE_ROLL_P .13
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0 # define RATE_ROLL_I 0.0
@ -337,7 +423,7 @@
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.12 # define RATE_PITCH_P 0.13
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.0 # define RATE_PITCH_I 0.0
@ -350,17 +436,17 @@
// YAW Control // YAW Control
// //
#ifndef STABILIZE_YAW_P #ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 4.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy # define STABILIZE_YAW_P 7 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif #endif
#ifndef STABILIZE_YAW_I #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 # define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif #endif
#ifndef STABILIZE_YAW_IMAX #ifndef STABILIZE_YAW_IMAX
# define STABILIZE_YAW_IMAX 15 // degrees * 100 # define STABILIZE_YAW_IMAX 8 // degrees * 100
#endif #endif
#ifndef RATE_YAW_P #ifndef RATE_YAW_P
# define RATE_YAW_P .15 // used to control response in turning # define RATE_YAW_P .13 // used to control response in turning
#endif #endif
#ifndef RATE_YAW_I #ifndef RATE_YAW_I
# define RATE_YAW_I 0.0 # define RATE_YAW_I 0.0
@ -382,53 +468,61 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Navigation control gains // Navigation control gains
// //
#ifndef LOITER_P
# define LOITER_P .4 //
#endif
#ifndef LOITER_I
# define LOITER_I 0.01 //
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 8 // degrees°
#endif
#ifndef NAV_P
# define NAV_P 2.4 // for 4.5 ms error = 13.5 pitch
#endif
#ifndef NAV_I
# define NAV_I 0.03 // this
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 8 // degrees
#endif
/*
#ifndef NAV_LOITER_P #ifndef NAV_LOITER_P
# define NAV_LOITER_P 2.4 //1.4 // # define NAV_LOITER_P .4 //1.4 //
#endif #endif
#ifndef NAV_LOITER_I #ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.01 // # define NAV_LOITER_I 0.05 //
#endif #endif
#ifndef NAV_LOITER_D #ifndef NAV_LOITER_D
# define NAV_LOITER_D 1.0 //1.4 // # define NAV_LOITER_D 2 //
#endif #endif
#ifndef NAV_LOITER_IMAX #ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 12 // degrees° # define NAV_LOITER_IMAX 8 // degrees°
#endif
#ifndef NAV_WP_P
# define NAV_WP_P 2.2 // for 4.5 ms error = 13.5 pitch
#endif
#ifndef NAV_WP_I
# define NAV_WP_I 0.06 // this
#endif
#ifndef NAV_WP_D
# define NAV_WP_D .5 //
#endif
#ifndef NAV_WP_IMAX
# define NAV_WP_IMAX 20 // degrees
#endif #endif
*/
#ifndef WAYPOINT_SPEED_MAX #ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph # define WAYPOINT_SPEED_MAX 300 // for 6m/s error = 13mph
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.35 // trying a lower val # define THROTTLE_P 0.4 // trying a lower val
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.01 //with 4m error, 12.5s windup # define THROTTLE_I 0.10 //with 4m error, 12.5s windup
#endif
#ifndef THROTTLE_D
# define THROTTLE_D 0.4 // upped with filter
#endif #endif
//#ifndef THROTTLE_D
//# define THROTTLE_D 0.6 // upped with filter
//#endif
#ifndef THROTTLE_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 30 # define THROTTLE_IMAX 40
#endif #endif

30
ArduCopterMega/control_modes.pde
View File

@ -2,19 +2,20 @@
static void read_control_switch() static void read_control_switch()
{ {
static bool switch_debouncer = false;
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
//motor_armed = (switchPosition < 5);
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
if(switch_debouncer){
// remember the prev location for GS
prev_WP = current_loc;
oldSwitchPosition = switchPosition;
switch_debouncer = false;
set_mode(flight_modes[switchPosition]); set_mode(flight_modes[switchPosition]);
}else{
oldSwitchPosition = switchPosition; switch_debouncer = true;
prev_WP = current_loc; }
// reset navigation integrators
// -------------------------
//reset_I();
} }
} }
@ -33,13 +34,6 @@ static void reset_control_switch()
{ {
oldSwitchPosition = -1; oldSwitchPosition = -1;
read_control_switch(); read_control_switch();
//SendDebug("MSG: reset_control_switch ");
//SendDebugln(oldSwitchPosition , DEC);
}
static void update_servo_switches()
{
} }
static boolean trim_flag; static boolean trim_flag;
@ -101,8 +95,8 @@ static void auto_trim()
static void trim_accel() static void trim_accel()
{ {
g.pid_stabilize_roll.reset_I(); g.pi_stabilize_roll.reset_I();
g.pid_stabilize_pitch.reset_I(); g.pi_stabilize_pitch.reset_I();
if(g.rc_1.control_in > 0){ if(g.rc_1.control_in > 0){
imu.ay(imu.ay() + 1); imu.ay(imu.ay() + 1);

24
ArduCopterMega/defines.h
View File

@ -4,6 +4,24 @@
#define ENABLED 1 #define ENABLED 1
#define DISABLED 0 #define DISABLED 0
// Flight modes
// ------------
#define YAW_HOLD 0
#define YAW_ACRO 1
#define YAW_AUTO 2
#define YAW_LOOK_AT_HOME 3
#define ROLL_PITCH_STABLE 0
#define ROLL_PITCH_ACRO 1
#define ROLL_PITCH_SIMPLE 2
#define ROLL_PITCH_AUTO 3
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
#define THROTTLE_AUTO 2
// active altitude sensor // active altitude sensor
// ---------------------- // ----------------------
#define SONAR 0 #define SONAR 0
@ -147,7 +165,7 @@
#define CH6_TOP_BOTTOM_RATIO 11 #define CH6_TOP_BOTTOM_RATIO 11
#define CH6_PMAX 12 #define CH6_PMAX 12
#define CH6_RELAY 13 #define CH6_RELAY 13
#define CH6_TRAVERSE_SPEED 14
// nav byte mask // nav byte mask
// ------------- // -------------
@ -306,7 +324,7 @@
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT #define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
#define BARO_FILTER_SIZE 6 //#define BARO_FILTER_SIZE 8
/* ************************************************************** */ /* ************************************************************** */
/* Expansion PIN's that people can use for various things. */ /* Expansion PIN's that people can use for various things. */
@ -352,7 +370,7 @@
// sonar // sonar
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters //#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
// Hardware Parameters // Hardware Parameters
#define SLIDE_SWITCH_PIN 40 #define SLIDE_SWITCH_PIN 40

1
ArduCopterMega/leds.pde
View File

@ -89,6 +89,7 @@ static void clear_leds()
digitalWrite(A_LED_PIN, LOW); digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW); digitalWrite(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW);
motor_light = false;
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
} }

126
ArduCopterMega/motors.pde
View File

@ -31,9 +31,17 @@ static void arm_motors()
motor_armed = true; motor_armed = true;
arming_counter = ARM_DELAY; arming_counter = ARM_DELAY;
// Clear throttle slew // Tune down DCM
// ------------------- // -------------------
//throttle_slew = 0; #if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.000006);
#endif
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
// Remember Orientation // Remember Orientation
// -------------------- // --------------------
@ -49,15 +57,17 @@ static void arm_motors()
startup_ground(); startup_ground();
} }
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// temp hack // temp hack
motor_light = true; motor_light = true;
digitalWrite(A_LED_PIN, HIGH); digitalWrite(A_LED_PIN, HIGH);
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
arming_counter++; arming_counter++;
} else{ } else{
arming_counter++; arming_counter++;
@ -73,13 +83,22 @@ static void arm_motors()
arming_counter = 0; arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){ }else if (arming_counter == DISARM_DELAY){
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
hil.send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); hil.send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif #endif
motor_armed = false; motor_armed = false;
arming_counter = DISARM_DELAY; arming_counter = DISARM_DELAY;
compass.save_offsets(); compass.save_offsets();
// Tune down DCM
// -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// tune up compass // tune up compass
// ----------------- // -----------------
dcm.kp_yaw(0.8); dcm.kp_yaw(0.8);
@ -117,92 +136,3 @@ set_servos_4()
output_motors_disarmed(); output_motors_disarmed();
} }
} }
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
//if (num++ > 25){
// num = 0;
//Serial.print("kP: ");
//Serial.println(g.pid_stabilize_roll.kP(),3);
//*/
/*
Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n",
(int)(dcm.yaw_sensor / 100),
lat_error,
long_error,
nav_lat,
nav_lon,
nav_roll,
nav_pitch,
cos_yaw_x,
sin_yaw_y,
target_bearing,
nav_bearing);
//*/
/*
gcs_simple.write_byte(control_mode);
//gcs_simple.write_int(motor_out[CH_1]);
//gcs_simple.write_int(motor_out[CH_2]);
//gcs_simple.write_int(motor_out[CH_3]);
//gcs_simple.write_int(motor_out[CH_4]);
gcs_simple.write_int(g.rc_3.servo_out);
gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
gcs_simple.write_int((int)nav_lat);
gcs_simple.write_int((int)nav_lon);
gcs_simple.write_int((int)nav_roll);
gcs_simple.write_int((int)nav_pitch);
//gcs_simple.write_int((int)(cos_yaw_x * 100));
//gcs_simple.write_int((int)(sin_yaw_y * 100));
gcs_simple.write_long(current_loc.lat); //28
gcs_simple.write_long(current_loc.lng); //32
gcs_simple.write_int((int)current_loc.alt); //34
gcs_simple.write_long(next_WP.lat);
gcs_simple.write_long(next_WP.lng);
gcs_simple.write_int((int)next_WP.alt); //44
gcs_simple.write_int((int)(target_bearing / 100));
gcs_simple.write_int((int)(nav_bearing / 100));
gcs_simple.write_int((int)(nav_yaw / 100));
if(altitude_sensor == BARO){
gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
}else{
gcs_simple.write_int((int)g.pid_sonar_throttle.get_integrator());
}
gcs_simple.write_int(g.throttle_cruise);
gcs_simple.write_int(g.throttle_cruise);
//24
gcs_simple.flush(10); // Message ID
//*/
//Serial.printf("\n tb %d\n", (int)(target_bearing / 100));
//Serial.printf("\n nb %d\n", (int)(nav_bearing / 100));
//Serial.printf("\n dcm %d\n", (int)(dcm.yaw_sensor / 100));
/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
current_loc.alt,
altitude_error,
(int)g.pid_baro_throttle.get_integrator(),
nav_throttle,
angle_boost());
*/
//}

248
ArduCopterMega/navigation.pde
View File

@ -31,9 +31,14 @@ static void navigate()
// -------------------------------------------- // --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP); target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction // nav_bearing will include xtrac correction
// ------------------------------------------ // -----------------------------------------
nav_bearing = target_bearing; xtrack_enabled = false;
if(xtrack_enabled){
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction());
}else{
nav_bearing = target_bearing;
}
} }
static bool check_missed_wp() static bool check_missed_wp()
@ -43,28 +48,10 @@ static bool check_missed_wp()
return (abs(temp) > 10000); //we pased the waypoint by 10 ° return (abs(temp) > 10000); //we pased the waypoint by 10 °
} }
static int
get_nav_throttle(long error)
{
int throttle;
// limit error to prevent I term run up
error = constrain(error, -600,600);
throttle = g.pid_throttle.get_pid(error, delta_ms_medium_loop, 1.0);
throttle = g.throttle_cruise + constrain(throttle, -80, 80);
// failed experiment
//int tem = alt_hold_velocity();
//throttle -= tem;
return throttle;
}
// ------------------------------ // ------------------------------
// long_error, lat_error // long_error, lat_error
static void calc_loiter_nav2() static void calc_location_error()
{ {
/* /*
Becuase we are using lat and lon to do our distance errors here's a quick chart: Becuase we are using lat and lon to do our distance errors here's a quick chart:
@ -80,180 +67,77 @@ static void calc_loiter_nav2()
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST
// Y PITCH // Y PITCH
lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH lat_error = next_WP.lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
// constrain input, not output to let I term ramp up and do it's job again wind
long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error
lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error
} }
// sets nav_lon, nav_lat #define NAV_ERR_MAX 400
static void calc_rate_nav2(int target_x_speed, int target_y_speed) static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
{ {
// find the rates: // moved to globals for logging
// calc the cos of the error to tell how fast we are moving towards the target in cm //int x_actual_speed, y_actual_speed;
int y_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); //int x_rate_error, y_rate_error;
int y_error = constrain(target_y_speed - y_speed, -1000, 1000); x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
// calc the sin of the error to tell how fast we are moving laterally to the target in cm float scaler = (float)max_speed/(float)NAV_ERR_MAX;
int x_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); g.pi_loiter_lat.kP(scaler);
int x_error = constrain(target_x_speed - x_speed, -1000, 1000); g.pi_loiter_lon.kP(scaler);
// how fast should we be going? int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
nav_lat += g.pid_nav_lat.get_pid(y_error, dTnav, 1.0); int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
nav_lat >>= 1; // divide by two for smooting
nav_lon += g.pid_nav_lon.get_pid(x_error, dTnav, 1.0); //Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed);
nav_lon >>= 1; // divide by two for smooting
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); if(x_target_speed > 0){
x_target_speed = max(x_target_speed, min_speed);
// limit our output
nav_lat = constrain(nav_lat, -3500, 3500); // +- max error
nav_lon = constrain(nav_lon, -3500, 3500); // +- max error
}
// ------------------------------
//nav_lon, nav_lat
static void calc_loiter_nav()
{
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m = 36 feet
1800 = 19.80m = 60 feet
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
*/
// X ROLL
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST
// Y PITCH
lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH
// constrain input, not output to let I term ramp up and do it's job again wind
long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error
lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750,
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
}
//nav_lat
static void calc_simple_nav()
{
// no dampening here in SIMPLE mode
nav_lat = constrain((wp_distance * 100), -4500, 4500); // +- 20m max error
// Scale response by kP
//nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
}
// sets nav_lon, nav_lat
static void calc_rate_nav(int speed)
{
// which direction are we moving?
long heading_error = nav_bearing - g_gps->ground_course;
heading_error = wrap_180(heading_error);
// calc the cos of the error to tell how fast we are moving towards the target in cm
int targetspeed = (float)g_gps->ground_speed * cos(radians((float)heading_error/100));
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
int lateralspeed = (float)g_gps->ground_speed * sin(radians((float)heading_error/100));
//targetspeed = max(targetspeed, 0);
// Reduce speed on RTL
if(control_mode == RTL){
int tmp = min(wp_distance, 80) * 50;
waypoint_speed = min(tmp, speed);
//waypoint_speed = max(waypoint_speed, 50);
}else{ }else{
int tmp = min(wp_distance, 200) * 90; x_target_speed = min(x_target_speed, -min_speed);
waypoint_speed = min(tmp, speed);
waypoint_speed = max(waypoint_speed, 50);
//waypoint_speed = g.waypoint_speed_max.get();
} }
int error = constrain(waypoint_speed - targetspeed, -1000, 1000); if(y_target_speed > 0){
y_target_speed = max(y_target_speed, min_speed);
}else{
y_target_speed = min(y_target_speed, -min_speed);
}
nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0); // find the rates:
nav_lat >>= 1; // divide by two for smooting // calc the cos of the error to tell how fast we are moving towards the target in cm
y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0));
y_rate_error = y_target_speed - y_actual_speed; // 413
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
nav_lon += lateralspeed * 2; // 2 is our fake PID gain //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
nav_lon >>= 1; // divide by two for smooting
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); // calc the sin of the error to tell how fast we are moving laterally to the target in cm
x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0));
// limit our output x_rate_error = x_target_speed - x_actual_speed;
nav_lat = constrain(nav_lat, -3500, 3500); // +- max error nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
}
// output pitch and roll
// ------------------------------
// nav_roll, nav_pitch
static void calc_loiter_output()
{
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x;
// BAD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * -1 = 1000 // roll right - Backwards
//EAST -1000 * 0 - 1000 * 1 = -1000 // roll left - Backwards
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
// GOOD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * 1 = -1000 // roll right
//EAST -1000 * 0 - 1000 * -1 = 1000 // roll left
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
nav_pitch = ((float)nav_lon * -cos_yaw_x + (float)nav_lat * sin_yaw_y);
// BAD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * -1 + 1000 * 0 = 1000 // pitch back - Backwards
//EAST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - Backwards
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
// GOOD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
} }
// nav_roll, nav_pitch // nav_roll, nav_pitch
static void calc_nav_output() static void calc_nav_pitch_roll()
{ {
// get the sin and cos of the bearing error - rotated 90°
float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
// rotate the vector // rotate the vector
//nav_roll = (float)nav_lat * cos_nav_x; nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
//nav_pitch = -(float)nav_lat * sin_nav_y; nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
nav_roll = (float)nav_lon * sin_nav_y - (float)nav_lat * -cos_nav_x;
nav_pitch = (float)nav_lon * cos_nav_x - (float)nav_lat * sin_nav_y; // flip pitch because forward is negative
nav_pitch = -nav_pitch;
} }
// ------------------------------ // ------------------------------
static void calc_bearing_error() static void calc_bearing_error()
{ {
// 83 99 Yaw = -16
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }
static void calc_altitude_error() static long get_altitude_error()
{ {
altitude_error = next_WP.alt - current_loc.alt; return next_WP.alt - current_loc.alt;
} }
/*
static void calc_altitude_smoothing_error() static void calc_altitude_smoothing_error()
{ {
// limit climb rates - we draw a straight line between first location and edge of waypoint_radius // limit climb rates - we draw a straight line between first location and edge of waypoint_radius
@ -268,26 +152,24 @@ static void calc_altitude_smoothing_error()
altitude_error = target_altitude - current_loc.alt; altitude_error = target_altitude - current_loc.alt;
} }
*/
static void update_loiter() static int get_loiter_angle()
{ {
float power; float power;
int angle;
if(wp_distance <= g.loiter_radius){ if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius); power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1); power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power)); angle = 90.0 * (2.0 + power);
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000; angle = power * 90;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
} }
nav_bearing = wrap_360(nav_bearing);
return angle;
} }
@ -305,25 +187,27 @@ static long wrap_180(long error)
return error; return error;
} }
static void update_crosstrack(void) static long get_crosstrack_correction(void)
{ {
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following
// Meters we are off track line // Meters we are off track line
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance;
// take meters * 100 to get adjustment to nav_bearing // take meters * 100 to get adjustment to nav_bearing
long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100; long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100;
nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing); // constrain answer to 30° to avoid overshoot
return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
} }
} }
static long cross_track_test() static long cross_track_test()
{ {
long temp = target_bearing - crosstrack_bearing; long temp = wrap_180(target_bearing - crosstrack_bearing);
temp = wrap_180(temp);
return abs(temp); return abs(temp);
} }

22
ArduCopterMega/read_commands.pde
View File

@ -79,26 +79,26 @@ static void parseCommand(char *buffer)
///* ///*
switch(cmd[0]){ switch(cmd[0]){
case 'P': case 'P':
g.pid_stabilize_roll.kP((float)value / 1000); g.pi_stabilize_roll.kP((float)value / 1000);
g.pid_stabilize_pitch.kP((float)value / 1000); g.pi_stabilize_pitch.kP((float)value / 1000);
g.pid_stabilize_pitch.save_gains(); g.pi_stabilize_pitch.save_gains();
break; break;
case 'I': case 'I':
g.pid_stabilize_roll.kI((float)value / 1000); g.pi_stabilize_roll.kI((float)value / 1000);
g.pid_stabilize_pitch.kI((float)value / 1000); g.pi_stabilize_pitch.kI((float)value / 1000);
g.pid_stabilize_pitch.save_gains(); g.pi_stabilize_pitch.save_gains();
break; break;
case 'D': case 'D':
//g.pid_stabilize_roll.kD((float)value / 1000); //g.pi_stabilize_roll.kD((float)value / 1000);
//g.pid_stabilize_pitch.kD((float)value / 1000); //g.pi_stabilize_pitch.kD((float)value / 1000);
break; break;
case 'X': case 'X':
g.pid_stabilize_roll.imax(value * 100); g.pi_stabilize_roll.imax(value * 100);
g.pid_stabilize_pitch.imax(value * 100); g.pi_stabilize_pitch.imax(value * 100);
g.pid_stabilize_pitch.save_gains(); g.pi_stabilize_pitch.save_gains();
break; break;
case 'R': case 'R':

24
ArduCopterMega/sensors.pde
View File

@ -12,9 +12,19 @@ static void init_barometer(void)
#endif #endif
ground_temperature = barometer.Temp; ground_temperature = barometer.Temp;
int i;
// We take some readings... // We take some readings...
for(int i = 0; i < 20; i++){ for(i = 0; i < 60; i++){
delay(20);
//read_baro_filtered();
// get new data from absolute pressure sensor
barometer.Read();
Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press);
}
for(i = 0; i < 20; i++){
delay(20); delay(20);
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
@ -22,8 +32,13 @@ static void init_barometer(void)
#endif #endif
// Get initial data from absolute pressure sensor // Get initial data from absolute pressure sensor
ground_pressure = read_baro_filtered(); //ground_pressure = read_baro_filtered();
// get new data from absolute pressure sensor
barometer.Read();
ground_pressure = barometer.Press;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure);
} }
abs_pressure = ground_pressure; abs_pressure = ground_pressure;
@ -34,12 +49,14 @@ static void init_barometer(void)
static long read_baro_filtered(void) static long read_baro_filtered(void)
{ {
long pressure = 0;
// get new data from absolute pressure sensor // get new data from absolute pressure sensor
barometer.Read(); barometer.Read();
return barometer.Press;
/*
long pressure = 0;
// add new data into our filter // add new data into our filter
baro_filter[baro_filter_index] = barometer.Press; baro_filter[baro_filter_index] = barometer.Press;
baro_filter_index++; baro_filter_index++;
@ -58,6 +75,7 @@ static long read_baro_filtered(void)
// average our sampels // average our sampels
return pressure /= BARO_FILTER_SIZE; return pressure /= BARO_FILTER_SIZE;
//*/
} }
static long read_barometer(void) static long read_barometer(void)

74
ArduCopterMega/setup.pde
View File

@ -824,75 +824,6 @@ static void report_radio()
print_blanks(2); print_blanks(2);
} }
/*
static void report_gains()
{
Serial.printf_P(PSTR("Gains\n"));
print_divider();
// Rate
Serial.printf_P(PSTR("Rate:\nroll:\n"));
print_PID(&g.pid_rate_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&g.pid_rate_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_rate_yaw);
// Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
print_PID(&g.pid_stabilize_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&g.pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_stabilize_yaw);
//Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
//Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
// Nav
Serial.printf_P(PSTR("Nav:\nlat:\n"));
print_PID(&g.pid_nav_lat);
Serial.printf_P(PSTR("long:\n"));
print_PID(&g.pid_nav_lon);
Serial.printf_P(PSTR("throttle:\n"));
print_PID(&g.pid_throttle);
print_blanks(2);
}
*/
/*static void report_xtrack()
{
Serial.printf_P(PSTR("XTrack\n"));
print_divider();
// radio
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"
"PITCH_MAX: %ld"),
(float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle,
(long)g.pitch_max);
print_blanks(2);
}
*/
/*static void report_throttle()
{
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d"),
(int)g.throttle_min,
(int)g.throttle_max,
(int)g.throttle_cruise,
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(2);
}*/
static void report_imu() static void report_imu()
{ {
Serial.printf_P(PSTR("IMU\n")); Serial.printf_P(PSTR("IMU\n"));
@ -991,12 +922,11 @@ static void report_gyro()
/***************************************************************************/ /***************************************************************************/
/*static void /*static void
print_PID(PID * pid) print_PID(PI * pid)
{ {
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"), Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"),
pid->kP(), pid->kP(),
pid->kI(), pid->kI(),
pid->kD(),
(long)pid->imax()); (long)pid->imax());
} }
*/ */

113
ArduCopterMega/system.pde
View File

@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "AC 2.0.39b Beta", main_menu_commands); MENU(main_menu, "AC 2.0.40 Beta", main_menu_commands);
#endif // CLI_ENABLED #endif // CLI_ENABLED
@ -256,7 +256,6 @@ static void init_ardupilot()
GPS_enabled = false; GPS_enabled = false;
//*
// Read in the GPS // Read in the GPS
for (byte counter = 0; ; counter++) { for (byte counter = 0; ; counter++) {
g_gps->update(); g_gps->update();
@ -270,7 +269,6 @@ static void init_ardupilot()
break; break;
} }
} }
//*/
// lengthen the idle timeout for gps Auto_detect // lengthen the idle timeout for gps Auto_detect
// --------------------------------------------- // ---------------------------------------------
@ -280,10 +278,6 @@ static void init_ardupilot()
// -------------------- // --------------------
report_gps(); report_gps();
// used to limit the input of error for loiter
// -------------------------------------------
loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP();
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground // read Baro pressure at ground
//----------------------------- //-----------------------------
@ -301,7 +295,7 @@ static void init_ardupilot()
//delay(100); //delay(100);
startup_ground(); startup_ground();
//Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max); //Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max);
Log_Write_Startup(); Log_Write_Startup();
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
@ -321,24 +315,28 @@ static void startup_ground(void)
report_imu(); report_imu();
#endif #endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// setup DCM for copters:
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif
// reset the leds // reset the leds
// --------------------------- // ---------------------------
clear_leds(); clear_leds();
} }
/*
#define YAW_HOLD 0
#define YAW_ACRO 1
#define YAW_AUTO 2
#define YAW_LOOK_AT_HOME 3
#define ROLL_PITCH_STABLE 0
#define ROLL_PITCH_ACRO 1
#define ROLL_PITCH_SIMPLE 2
#define ROLL_PITCH_AUTO 3
#define THROTTLE_MANUAL 0
#define THROTTLE_HOLD 1
#define THROTTLE_AUTO 2
*/
static void set_mode(byte mode) static void set_mode(byte mode)
{ {
if(control_mode == mode){ if(control_mode == mode){
@ -346,13 +344,6 @@ static void set_mode(byte mode)
return; return;
} }
// XXX
Serial.printf_P(PSTR("\nRAM: %lu\n"), freeRAM());
// reset the Nav_WP I term
g.pid_nav_wp.reset_I();
old_control_mode = control_mode; old_control_mode = control_mode;
control_mode = mode; control_mode = mode;
@ -365,46 +356,94 @@ static void set_mode(byte mode)
motor_auto_armed = false; motor_auto_armed = false;
} }
//send_text_P(SEVERITY_LOW,PSTR("control mode"));
//Serial.printf("set mode: %d\n",control_mode);
Serial.println(flight_mode_strings[control_mode]); Serial.println(flight_mode_strings[control_mode]);
// report the GPS and Motor arming status
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction
xtrack_enabled = false;
switch(control_mode) switch(control_mode)
{ {
case ACRO: case ACRO:
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL;
reset_nav_I();
break;
case STABILIZE:
yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_STABLE;
throttle_mode = THROTTLE_MANUAL;
reset_nav_I(); reset_nav_I();
break; break;
case SIMPLE: case SIMPLE:
case STABILIZE: yaw_mode = SIMPLE_YAW;
do_loiter_at_location(); roll_pitch_mode = SIMPLE_RP;
throttle_mode = SIMPLE_THR;
reset_nav_I(); reset_nav_I();
break; break;
case ALT_HOLD: case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR;
init_throttle_cruise(); init_throttle_cruise();
do_loiter_at_location();
break; break;
case AUTO: case AUTO:
reset_nav_I();
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR;
init_throttle_cruise(); init_throttle_cruise();
// loads the commands from where we left off
init_auto(); init_auto();
// do crosstrack correction
xtrack_enabled = true;
break; break;
case CIRCLE: case CIRCLE:
case LOITER: yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR;
init_throttle_cruise(); init_throttle_cruise();
do_loiter_at_location(); next_WP = current_loc;
break;
case LOITER:
yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR;
init_throttle_cruise();
next_WP = current_loc;
break; break;
case GUIDED: case GUIDED:
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
xtrack_enabled = true;
init_throttle_cruise(); init_throttle_cruise();
set_next_WP(&guided_WP); next_WP = current_loc;
break; break;
case RTL: case RTL:
yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
xtrack_enabled = true;
init_throttle_cruise(); init_throttle_cruise();
do_RTL(); do_RTL();
break; break;
@ -497,7 +536,7 @@ init_throttle_cruise()
{ {
// are we moving from manual throttle to auto_throttle? // are we moving from manual throttle to auto_throttle?
if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){ if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){
g.pid_throttle.reset_I(); g.pi_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in); g.throttle_cruise.set_and_save(g.rc_3.control_in);
} }
} }

70
ArduCopterMega/test.pde
View File

@ -210,68 +210,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
/*
static int8_t
test_wp_nav(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
dTnav = 200;
current_loc.lat = 32.9513090 * t7;
current_loc.lng = -117.2381700 * t7;
do_loiter_at_location();
wp_control = LOITER_MODE;
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
while(1){
read_radio();
delay(dTnav);
current_loc.lng = (-117.2381700 * t7) + g.rc_1.control_in / 2;
current_loc.lat = (32.9513090 * t7) + g.rc_2.control_in / 2;
navigate();
update_nav_wp();
Serial.printf("Lon_e: %ld, nLon, %ld, Lat_e %ld, nLat %ld\n", long_error, nav_lon, lat_error, nav_lat);
if(Serial.available() > 0){
return (0);
}
}
}
//*/
/*
{
print_hit_enter();
delay(1000);
g.rc_6.set_range(0,900);
g.rc_4.set_angle(9000);
dTnav = 200;
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
while(1){
delay(20);
read_radio();
target_bearing = 0;
g_gps->ground_course = g.rc_4.control_in;
g_gps->ground_speed = g.rc_6.control_in;
calc_rate_nav();
Serial.printf(" gps_GC: %ld, gps_GS: %d\n", g_gps->ground_course, g_gps->ground_speed);
if(Serial.available() > 0){
return (0);
}
}
}
*/
static int8_t static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv) test_failsafe(uint8_t argc, const Menu::arg *argv)
{ {
@ -340,7 +278,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
init_rc_in(); init_rc_in();
control_mode = STABILIZE; control_mode = STABILIZE;
Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP()); Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_auto_armed = false; motor_auto_armed = false;
@ -509,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
accels.x, accels.y, accels.z, accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.z); gyros.x, gyros.y, gyros.z);
*/ */
/* ///*
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"), Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
cos_pitch_x, cos_pitch_x,
sin_pitch_y, sin_pitch_y,
@ -736,6 +674,9 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED
Serial.printf_P(PSTR("traverse: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Extras //Extras
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
@ -874,6 +815,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
delay(100); delay(100);
baro_alt = read_barometer(); baro_alt = read_barometer();
Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt); Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt);
//Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2);
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
} }