aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

23
ArduCopterMega/APM_Config.h
View File

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

531
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_IMU.h> // ArduPilot Mega IMU 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 <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
@ -261,6 +261,13 @@ static const char* flight_mode_strings[] = {
//Vector3f accels_rot;
//float accel_gain = 20;
// temp
int y_actual_speed;
int y_rate_error;
// calc the
int x_actual_speed;
int x_rate_error;
// Radio
// -----
@ -289,8 +296,6 @@ static boolean motor_auto_armed; // if true,
//int max_stabilize_dampener; //
//int max_yaw_dampener; //
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;
// LED output
@ -314,6 +319,8 @@ static const float radius_of_earth = 6378100; // meters
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 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 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?
@ -336,6 +343,7 @@ static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static bool simple_bearing_is_set = false;
static long initial_simple_bearing; // used for Simple mode
// Acro
#if CH7_OPTION == DO_FLIP
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 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 yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging
static int loiter_error_max;
// Battery Sensors
// ---------------
@ -367,16 +374,11 @@ static float current_amps;
static float current_total;
static bool low_batt = false;
// Airspeed Sensors
// ----------------
// Barometer Sensor variables
// --------------------------
static long abs_pressure;
static long ground_pressure;
static int ground_temperature;
static int32_t baro_filter[BARO_FILTER_SIZE];
static byte baro_filter_index;
// Altitude Sensor variables
// ----------------------
@ -384,18 +386,20 @@ static int sonar_alt;
static int baro_alt;
static int baro_alt_offset;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate;
// 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 land_complete;
//static int takeoff_altitude;
static int landing_distance; // meters;
static long old_alt; // used for managing altitude rates
static int velocity_land;
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
// -----------------
@ -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_pitch; // deg * 100 : target pitch 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_lon; // for error calcs
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 set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -484,7 +489,6 @@ static int G_Dt_max; // Max main loop cycle time in milliseconds
static int gps_fix_count;
static byte gcs_messages_sent;
// GCS
// ---
static char GCS_buffer[53];
@ -506,7 +510,8 @@ static uint8_t delta_ms_fiftyhz;
static byte slow_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
@ -610,7 +615,9 @@ static void fast_loop()
// 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
// ------------------------------
@ -694,13 +701,13 @@ static void medium_loop()
// Read altitude from sensors
// --------------------------
update_alt();
update_altitude();
// altitude smoothing
// ------------------
//calc_altitude_smoothing_error();
calc_altitude_error();
altitude_error = get_altitude_error();
// invalidate the throttle hold value
// ----------------------------------
@ -1091,8 +1098,42 @@ void update_location(void)
}
#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 (do_flip){
@ -1101,221 +1142,87 @@ void update_current_flight_mode(void)
}
#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){
default:
// 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
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:
switch(roll_pitch_mode){
case ROLL_PITCH_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);
return;
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);
case ROLL_PITCH_STABLE:
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
break;
case SIMPLE:
flight_timer++;
case ROLL_PITCH_SIMPLE:
simple_timer++;
if(simple_timer > 5){
simple_timer = 0;
if(flight_timer > 6){
flight_timer = 0;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
// make sure this is always 0
simple_WP.lat = 0;
simple_WP.lng = 0;
// pre-calc rotation (stored in real degrees)
// roll
float cos_x = sin(radians(90 - delta));
// pitch
float sin_y = cos(radians(90 - delta));
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
// Rotate input by the initial bearing
// roll
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;
// 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();
case ROLL_PITCH_AUTO:
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
break;
}
/*
#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);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(nav_pitch);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
}
// Throttle control
void update_throttle_mode(void)
{
switch(throttle_mode){
case THROTTLE_MANUAL:
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);
break;
case ALT_HOLD:
case THROTTLE_HOLD:
// allow interactive changing of atitude
adjust_altitude();
// 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
// fall through
// 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
case THROTTLE_AUTO:
if(invalid_throttle){
auto_throttle();
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 150); //150 = target speed of 1.5m/s
// apply throttle control
g.rc_3.servo_out = get_throttle(nav_throttle);
// clear the new data flag
invalid_throttle = false;
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0);
break;
case CIRCLE:
case GUIDED:
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
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, 0.25);
break;
case LOITER:
// 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
adjust_altitude();
// 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
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, 1.0);
break;
default:
//Serial.print("$");
break;
}
}
}
@ -1331,7 +1238,7 @@ static void update_navigation()
// note: wp_control is handled by commands_logic
// calculates desired Yaw
update_nav_yaw();
update_auto_yaw();
// calculates the desired Roll and Pitch
update_nav_wp();
@ -1339,40 +1246,26 @@ static void update_navigation()
case GUIDED:
case RTL:
if(wp_distance > 5){
if(wp_distance > 4){
// calculates desired Yaw
// XXX this is an experiment
#if FRAME_CONFIG == HELI_FRAME
update_nav_yaw();
update_auto_yaw();
#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;
// switch passthrough to LOITER
case LOITER:
// are we Traversing or Loitering?
//wp_control = (wp_distance < 20) ? LOITER_MODE : WP_MODE;
wp_control = LOITER_MODE;
// calculates the desired Roll and Pitch
@ -1381,20 +1274,43 @@ static void update_navigation()
case CIRCLE:
yaw_tracking = MAV_ROI_WPNEXT;
// calculates desired Yaw
update_nav_yaw();
update_auto_yaw();
// hack to elmininate crosstrack effect
crosstrack_bearing = target_bearing;
// calc the lat and long error to the target
calc_location_error();
// get a new nav_bearing
update_loiter();
// use error as the desired rate towards the target
// 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
calc_rate_nav(400);
// rotate nav_lat and nav_long by nav_bearing so we circle the target
{
// 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
calc_nav_output();
// pre-calc rotation (stored in real degrees)
// 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;
}
@ -1430,12 +1346,19 @@ static void update_trig(void){
cos_roll_x = temp.c.z / 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();
//accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
}
// updated at 10hz
static void update_alt()
static void update_altitude()
{
altitude_sensor = BARO;
@ -1452,6 +1375,14 @@ static void update_alt()
baro_alt = read_barometer();
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 = constrain(scale, 0, 1);
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;
}
altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer
old_altitude = current_loc.alt;
#endif
}
static void
adjust_altitude()
{
flight_timer++;
if(flight_timer >= 2){
flight_timer = 0;
alt_timer++;
if(alt_timer >= 2){
alt_timer = 0;
if(g.rc_3.control_in <= 200){
next_WP.alt -= 3; // 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 -= 1; // 1 meter per second
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){
next_WP.alt += 4; // 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 + 600)); // don't go more than 4 meters below current location
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // 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
#if CHANNEL_6_TUNING == CH6_STABILIZE_KP
g.pid_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_roll.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
g.pid_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_roll.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
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
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
#elif CHANNEL_6_TUNING == CH6_RATE_KP
g.pid_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_roll.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
g.pid_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_roll.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
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
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
#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
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
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
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
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();
#endif
}
static void update_nav_wp()
@ -1551,74 +1484,46 @@ static void update_nav_wp()
// XXX Guided mode!!!
if(wp_control == LOITER_MODE){
#if LOITER_TEST == 0
// 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
calc_loiter_output();
#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
calc_nav_pitch_roll();
} else {
// how far are we from the ideal trajectory?
// this pushes us back on course
update_crosstrack();
// for long journey's reset the wind resopnse
// it assumes we are standing still.
g.pi_loiter_lat.reset_I();
g.pi_loiter_lat.reset_I();
// calc a rate dampened pitch to the target
calc_rate_nav(g.waypoint_speed_max.get());
// calc the lat and long error to the target
calc_location_error();
// rotate that pitch to the copter frame of reference
calc_nav_output();
// use error as the desired rate towards the target
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.
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){
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);
}
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);
return get_bearing(&current_loc, &home);
}

75
ArduCopterMega/Attitude.pde
View File

@ -13,13 +13,13 @@ get_stabilize_roll(long target_angle)
error = constrain(error, -2500, 2500);
// 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);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
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);
#endif
@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle)
error = constrain(error, -2500, 2500);
// 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);
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
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);
#endif
@ -54,32 +54,31 @@ get_stabilize_pitch(long target_angle)
return (int)constrain(rate, -2500, 2500);
}
#define YAW_ERROR_MAX 2000
static int
get_stabilize_yaw(long target_angle, float scaler)
get_stabilize_yaw(long target_angle)
{
long error;
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
error = constrain(error, -4500, 4500);
// desired Rate:
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler);
yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
rate = g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop);
//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( ! g.heli_ext_gyro_enabled ) {
// Rate P:
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
// Rate P:
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);
#endif
@ -87,6 +86,26 @@ get_stabilize_yaw(long target_angle, float scaler)
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
get_rate_roll(long target_rate)
{
@ -94,7 +113,7 @@ get_rate_roll(long target_rate)
target_rate = constrain(target_rate, -2500, 2500);
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:
return (int)constrain(target_rate, -2500, 2500);
@ -107,7 +126,7 @@ get_rate_pitch(long target_rate)
target_rate = constrain(target_rate, -2500, 2500);
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:
return (int)constrain(target_rate, -2500, 2500);
@ -119,7 +138,7 @@ get_rate_yaw(long target_rate)
long error;
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:
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.
// Keeps outdated data out of our calculations
static void
reset_nav_I(void)
static void reset_nav_I(void)
{
g.pid_nav_lat.reset_I();
g.pid_nav_lon.reset_I();
g.pid_nav_wp.reset_I();
g.pid_crosstrack.reset_I();
g.pid_throttle.reset_I();
// I removed these, they don't seem to be needed.
g.pi_loiter_lat.reset_I();
g.pi_loiter_lat.reset_I();
g.pi_nav_lat.reset_I();
g.pi_nav_lon.reset_I();
g.pi_crosstrack.reset_I();
g.pi_throttle.reset_I();
}
@ -146,8 +166,7 @@ throttle control
// user input:
// -----------
static int
get_throttle(int throttle_input)
static int get_throttle(int throttle_input)
{
throttle_input = (float)throttle_input * angle_boost();
//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()
{
@ -185,7 +205,8 @@ static int alt_hold_velocity()
error = min(error, 200);
error = 1 - (error/ 200.0);
return (accels_rot.z + 9.81) * accel_gain * error;
}*/
}
*/
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 0x22: templong = bearing_error; 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 0x26: templong = wp_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
dump_log = argv[1].i;
if(dump_log == 99){
Log_Read(1, 4096);
}
if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
if (/*(argc != 2) || */ (dump_log < 1)) {
Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
Log_Read(1, 4095);
Log_Read(0, 4095);
erase_logs(NULL, NULL);
return(-1);
}
@ -139,6 +135,14 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j);
clear_header();
Serial.printf_P(PSTR("\nLog erased.\n"));
return (0);
}
static void clear_header()
{
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
@ -146,8 +150,6 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
DataFlash.WriteByte(0);
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n"));
return (0);
}
static int8_t
@ -537,11 +539,19 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)(target_bearing/100)); // 3
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)lat_error); // 6
DataFlash.WriteInt((int)nav_lon); // 7
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);
}
@ -550,7 +560,9 @@ static void Log_Read_Nav_Tuning()
{
// 1 2 3 4
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.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()); //nav bearing
}
@ -576,11 +596,9 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw
DataFlash.WriteByte(yaw_debug);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error/100);
DataFlash.WriteInt((int)(omega.z * 100));
// Alt hold
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)altitude_error); //
DataFlash.WriteInt((int)g.pid_throttle.get_integrator());
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());
DataFlash.WriteByte(END_BYTE);
}
@ -598,9 +616,11 @@ static void Log_Write_Control_Tuning()
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
Serial.printf_P(PSTR( "CTUN, %d, %d, "
"%d, %d, %d, %d, %1.4f, "
"%d, %d, %d, %d, %d, %d\n"),
Serial.printf_P(PSTR( "CTUN, "
"%d, %d, "
"%d, %d, %ld, "
"%d, %d, %ld, "
"%d, %d, %ld\n"),
// Control
DataFlash.ReadInt(),
@ -611,7 +631,8 @@ static void Log_Read_Control_Tuning()
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(float)DataFlash.ReadInt(),// Gyro Rate
//(float)DataFlash.ReadInt(),// Gyro Rate
DataFlash.ReadLong(),
// Alt Hold
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,
altitude_error / 1.0e2,
0,
crosstrack_error);
0);
//}
break;
}

85
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 106;
static const uint16_t k_format_version = 107;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -151,23 +151,20 @@ public:
k_param_waypoint_speed_max,
//
// 240: PID Controllers
// 240: PI/D Controllers
//
// Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pid_rate_roll = 240,
k_param_pid_rate_pitch,
k_param_pid_rate_yaw,
k_param_pid_stabilize_roll,
k_param_pid_stabilize_pitch,
k_param_pid_stabilize_yaw,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pid_nav_wp,
k_param_pid_throttle,
k_param_pid_crosstrack,
k_param_pi_rate_roll = 240,
k_param_pi_rate_pitch,
k_param_pi_rate_yaw,
k_param_pi_stabilize_roll,
k_param_pi_stabilize_pitch,
k_param_pi_stabilize_yaw,
k_param_pi_loiter_lat,
k_param_pi_loiter_lon,
k_param_pi_nav_lat,
k_param_pi_nav_lon,
k_param_pi_throttle,
k_param_pi_crosstrack,
// 254,255: reserved
@ -260,18 +257,23 @@ public:
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
// PID controllers
PID pid_rate_roll;
PID pid_rate_pitch;
PID pid_rate_yaw;
PID pid_stabilize_roll;
PID pid_stabilize_pitch;
PID pid_stabilize_yaw;
PID pid_nav_lat;
PID pid_nav_lon;
PID pid_nav_wp;
PID pid_throttle;
PID pid_crosstrack;
// PI/D controllers
APM_PI pi_rate_roll;
APM_PI pi_rate_pitch;
APM_PI pi_rate_yaw;
APM_PI pi_stabilize_roll;
APM_PI pi_stabilize_pitch;
APM_PI pi_stabilize_yaw;
APM_PI pi_loiter_lat;
APM_PI pi_loiter_lon;
APM_PI pi_nav_lat;
APM_PI pi_nav_lon;
APM_PI pi_throttle;
APM_PI pi_crosstrack;
uint8_t junk;
@ -355,23 +357,24 @@ public:
rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")),
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),
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, 0, 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_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100),
pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100),
pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100),
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, 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),
pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, STABILIZE_YAW_IMAX * 100),
pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100),
pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100),
pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_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),
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
pi_loiter_lat (k_param_pi_loiter_lat, PSTR("LOITER_LAT_"),LOITER_P, LOITER_I, LOITER_IMAX * 100),
pi_loiter_lon (k_param_pi_loiter_lon, PSTR("LOITER_LON_"),LOITER_P, LOITER_I, LOITER_IMAX * 100),
pid_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, XTRACK_IMAX),
pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100),
pi_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
{

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
prev_WP = home;
next_WP = home;
}

5
ArduCopterMega/commands_logic.pde
View File

@ -5,9 +5,6 @@
/********************************************************************************/
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){
case MAV_CMD_NAV_TAKEOFF:
@ -203,6 +200,7 @@ static void do_RTL(void)
temp.alt = read_alt_to_hold();
//so we know where we are navigating from
// --------------------------------------
next_WP = current_loc;
// Loads WP from Memory
@ -210,6 +208,7 @@ static void do_RTL(void)
set_next_WP(&temp);
// output control mode to the ground station
// -----------------------------------------
gcs.send_message(MSG_HEARTBEAT);
}

3
ArduCopterMega/commands_process.pde
View File

@ -67,9 +67,6 @@ static void verify_commands(void)
if(verify_must()){
//Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND;
// reset rate controlled nav
g.pid_nav_wp.reset_I();
}else{
//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
//
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.0
# define STABILIZE_ROLL_P 3.6
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025
# define STABILIZE_ROLL_I 0.02
#endif
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 10 // degrees
# define STABILIZE_ROLL_IMAX 1.5 // degrees
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.0
# define STABILIZE_PITCH_P 3.6
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.025
# define STABILIZE_PITCH_I 0.02
#endif
#ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 10 // degrees
# define STABILIZE_PITCH_IMAX 1.5 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Rate Control
//
#ifndef RATE_ROLL_P
# define RATE_ROLL_P .12
# define RATE_ROLL_P .13
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0
@ -337,7 +423,7 @@
#endif
#ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.12
# define RATE_PITCH_P 0.13
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.0
@ -350,17 +436,17 @@
// YAW Control
//
#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
#ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef STABILIZE_YAW_IMAX
# define STABILIZE_YAW_IMAX 15 // degrees * 100
# define STABILIZE_YAW_IMAX 8 // degrees * 100
#endif
#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
#ifndef RATE_YAW_I
# define RATE_YAW_I 0.0
@ -382,53 +468,61 @@
//////////////////////////////////////////////////////////////////////////////
// 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
# define NAV_LOITER_P 2.4 //1.4 //
# define NAV_LOITER_P .4 //1.4 //
#endif
#ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.01 //
# define NAV_LOITER_I 0.05 //
#endif
#ifndef NAV_LOITER_D
# define NAV_LOITER_D 1.0 //1.4 //
# define NAV_LOITER_D 2 //
#endif
#ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 12 // 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
# define NAV_LOITER_IMAX 8 // degrees°
#endif
*/
#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
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THROTTLE_P
# define THROTTLE_P 0.35 // trying a lower val
# define THROTTLE_P 0.4 // trying a lower val
#endif
#ifndef THROTTLE_I
# define THROTTLE_I 0.01 //with 4m error, 12.5s windup
#endif
#ifndef THROTTLE_D
# define THROTTLE_D 0.4 // upped with filter
# define THROTTLE_I 0.10 //with 4m error, 12.5s windup
#endif
//#ifndef THROTTLE_D
//# define THROTTLE_D 0.6 // upped with filter
//#endif
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 30
# define THROTTLE_IMAX 40
#endif

28
ArduCopterMega/control_modes.pde
View File

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

24
ArduCopterMega/defines.h
View File

@ -4,6 +4,24 @@
#define ENABLED 1
#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
// ----------------------
#define SONAR 0
@ -147,7 +165,7 @@
#define CH6_TOP_BOTTOM_RATIO 11
#define CH6_PMAX 12
#define CH6_RELAY 13
#define CH6_TRAVERSE_SPEED 14
// nav byte mask
// -------------
@ -306,7 +324,7 @@
#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 BARO_FILTER_SIZE 6
//#define BARO_FILTER_SIZE 8
/* ************************************************************** */
/* Expansion PIN's that people can use for various things. */
@ -352,7 +370,7 @@
// 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
#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(B_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
motor_light = false;
led_mode = NORMAL_LEDS;
}

122
ArduCopterMega/motors.pde
View File

@ -31,9 +31,17 @@ static void arm_motors()
motor_armed = true;
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
// --------------------
@ -49,15 +57,17 @@ static void arm_motors()
startup_ground();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
// temp hack
motor_light = true;
digitalWrite(A_LED_PIN, HIGH);
// tune down compass
// -----------------
dcm.kp_yaw(0.08);
dcm.ki_yaw(0);
arming_counter++;
} else{
arming_counter++;
@ -76,10 +86,19 @@ static void arm_motors()
#if HIL_MODE != HIL_MODE_DISABLED
hil.send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
#endif
motor_armed = false;
arming_counter = DISARM_DELAY;
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
// -----------------
dcm.kp_yaw(0.8);
@ -117,92 +136,3 @@ set_servos_4()
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());
*/
//}

258
ArduCopterMega/navigation.pde
View File

@ -31,10 +31,15 @@ static void navigate()
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction
// ------------------------------------------
// nav_bearing will include xtrac correction
// -----------------------------------------
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()
{
@ -43,28 +48,10 @@ static bool check_missed_wp()
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
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:
@ -80,180 +67,77 @@ static void calc_loiter_nav2()
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
lat_error = next_WP.lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
}
// sets nav_lon, nav_lat
static void calc_rate_nav2(int target_x_speed, int target_y_speed)
#define NAV_ERR_MAX 400
static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
{
// moved to globals for logging
//int x_actual_speed, y_actual_speed;
//int x_rate_error, y_rate_error;
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
float scaler = (float)max_speed/(float)NAV_ERR_MAX;
g.pi_loiter_lat.kP(scaler);
g.pi_loiter_lon.kP(scaler);
int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
//Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed);
if(x_target_speed > 0){
x_target_speed = max(x_target_speed, min_speed);
}else{
x_target_speed = min(x_target_speed, -min_speed);
}
if(y_target_speed > 0){
y_target_speed = max(y_target_speed, min_speed);
}else{
y_target_speed = min(y_target_speed, -min_speed);
}
// find the rates:
// calc the cos of the error to tell how fast we are moving towards the target in cm
int y_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0));
int y_error = constrain(target_y_speed - y_speed, -1000, 1000);
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);
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
int x_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0));
int x_error = constrain(target_x_speed - x_speed, -1000, 1000);
// how fast should we be going?
nav_lat += g.pid_nav_lat.get_pid(y_error, dTnav, 1.0);
nav_lat >>= 1; // divide by two for smooting
nav_lon += g.pid_nav_lon.get_pid(x_error, dTnav, 1.0);
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);
// 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{
int tmp = min(wp_distance, 200) * 90;
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);
nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0);
nav_lat >>= 1; // divide by two for smooting
nav_lon += lateralspeed * 2; // 2 is our fake PID gain
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);
// limit our output
nav_lat = constrain(nav_lat, -3500, 3500); // +- max error
}
// 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
x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0));
x_rate_error = x_target_speed - x_actual_speed;
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
}
// 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
//nav_roll = (float)nav_lat * cos_nav_x;
//nav_pitch = -(float)nav_lat * sin_nav_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;
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;
}
// ------------------------------
static void calc_bearing_error()
{
// 83 99 Yaw = -16
bearing_error = nav_bearing - dcm.yaw_sensor;
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()
{
// 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;
}
*/
static void update_loiter()
static int get_loiter_angle()
{
float power;
int angle;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
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)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
angle = power * 90;
}
nav_bearing = wrap_360(nav_bearing);
return angle;
}
@ -305,25 +187,27 @@ static long wrap_180(long error)
return error;
}
static void update_crosstrack(void)
static long get_crosstrack_correction(void)
{
// Crosstrack Error
// ----------------
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
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
long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100;
nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100;
// 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()
{
long temp = target_bearing - crosstrack_bearing;
temp = wrap_180(temp);
long temp = wrap_180(target_bearing - crosstrack_bearing);
return abs(temp);
}

22
ArduCopterMega/read_commands.pde
View File

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

24
ArduCopterMega/sensors.pde
View File

@ -12,9 +12,19 @@ static void init_barometer(void)
#endif
ground_temperature = barometer.Temp;
int i;
// 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);
#if HIL_MODE == HIL_MODE_SENSORS
@ -22,8 +32,13 @@ static void init_barometer(void)
#endif
// 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;
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;
@ -34,12 +49,14 @@ static void init_barometer(void)
static long read_baro_filtered(void)
{
long pressure = 0;
// get new data from absolute pressure sensor
barometer.Read();
return barometer.Press;
/*
long pressure = 0;
// add new data into our filter
baro_filter[baro_filter_index] = barometer.Press;
baro_filter_index++;
@ -58,6 +75,7 @@ static long read_baro_filtered(void)
// average our sampels
return pressure /= BARO_FILTER_SIZE;
//*/
}
static long read_barometer(void)

74
ArduCopterMega/setup.pde
View File

@ -824,75 +824,6 @@ static void report_radio()
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()
{
Serial.printf_P(PSTR("IMU\n"));
@ -991,12 +922,11 @@ static void report_gyro()
/***************************************************************************/
/*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->kI(),
pid->kD(),
(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.
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
@ -256,7 +256,6 @@ static void init_ardupilot()
GPS_enabled = false;
//*
// Read in the GPS
for (byte counter = 0; ; counter++) {
g_gps->update();
@ -270,7 +269,6 @@ static void init_ardupilot()
break;
}
}
//*/
// lengthen the idle timeout for gps Auto_detect
// ---------------------------------------------
@ -280,10 +278,6 @@ static void init_ardupilot()
// --------------------
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
// read Baro pressure at ground
//-----------------------------
@ -301,7 +295,7 @@ static void init_ardupilot()
//delay(100);
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();
SendDebug("\nReady to FLY ");
@ -321,24 +315,28 @@ static void startup_ground(void)
report_imu();
#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
// ---------------------------
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)
{
if(control_mode == mode){
@ -346,13 +344,6 @@ static void set_mode(byte mode)
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;
control_mode = mode;
@ -365,46 +356,94 @@ static void set_mode(byte mode)
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]);
// report the GPS and Motor arming status
led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction
xtrack_enabled = false;
switch(control_mode)
{
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();
break;
case SIMPLE:
case STABILIZE:
do_loiter_at_location();
yaw_mode = SIMPLE_YAW;
roll_pitch_mode = SIMPLE_RP;
throttle_mode = SIMPLE_THR;
reset_nav_I();
break;
case ALT_HOLD:
yaw_mode = ALT_HOLD_YAW;
roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR;
init_throttle_cruise();
do_loiter_at_location();
break;
case AUTO:
reset_nav_I();
yaw_mode = AUTO_YAW;
roll_pitch_mode = AUTO_RP;
throttle_mode = AUTO_THR;
init_throttle_cruise();
// loads the commands from where we left off
init_auto();
// do crosstrack correction
xtrack_enabled = true;
break;
case CIRCLE:
case LOITER:
yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR;
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;
case GUIDED:
yaw_mode = YAW_AUTO;
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
xtrack_enabled = true;
init_throttle_cruise();
set_next_WP(&guided_WP);
next_WP = current_loc;
break;
case RTL:
yaw_mode = RTL_YAW;
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
xtrack_enabled = true;
init_throttle_cruise();
do_RTL();
break;
@ -497,7 +536,7 @@ init_throttle_cruise()
{
// are we moving from manual throttle to auto_throttle?
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);
}
}

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
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();
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);
motor_auto_armed = false;
@ -509,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
accels.x, accels.y, accels.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"),
cos_pitch_x,
sin_pitch_y,
@ -736,6 +674,9 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
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
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
@ -874,6 +815,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
delay(100);
baro_alt = read_barometer();
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){
return (0);
}