Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

New PIDs - I rewrote the control laws from scratch to add a PI Rate function. The end result should fly nearly identically to the current version. The nice detail is that we can use NG PID values for easy transition!

Browse Source 
Before: ->  After
Stabilize P –> Stabilize P (Use NG values, or 8.3 x the older AC2 value)
Stabilize I –> Stabilize I (Stays same value)
Stabilize D –> Rate P (Stays same value)
–> Rate I (new)
 
Added a new value – an I term for rate. The old stabilization routines did not use this term. Please refer to the config.h file to read more about the new PIDs.
Added framework for using DCM corrected Accelerometer rates. Code is commented out for now.
Added set home at Arming.
Crosstrack is now a full PID loop, rather than just a P gain for more control. 
Throttle now slews when switching out of Alt hold or Auto modes for less jarring transitions
Sonar and Baro PIDs are now combined into a throttle PID Yaw control is completely re-written.
Added Octa_Quad support - Max



git-svn-id: https://arducopter.googlecode.com/svn/trunk@2836 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-11 00:47:08 +00:00
parent 165643c307
commit 35bf288abd
18 changed files with 1017 additions and 1002 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopterMega/APM_Config.h
View File

@ -29,25 +29,21 @@
#define CHANNEL_6_TUNING CH6_NONE #define CHANNEL_6_TUNING CH6_NONE
/* /*
CH6_NONE CH6_NONE
CH6_STABLIZE_KP CH6_STABILIZE_KP
CH6_STABLIZE_KD CH6_STABILIZE_KI
CH6_BARO_KP CH6_RATE_KP
CH6_BARO_KD CH6_RATE_KI
CH6_SONAR_KP CH6_THROTTLE_KP
CH6_SONAR_KD CH6_THROTTLE_KD
CH6_Y6_SCALING CH6_YAW_KP
CH6_YAW_KI
CH6_YAW_RATE_KP
CH6_YAW_RATE_KI
CH6_TOP_BOTTOM_RATIO
CH6_PMAX
*/ */
//#define ACRO_RATE_TRIGGER 4200
// if you want full ACRO mode, set value to 0
// if you want mostly stabilize with flips, set value to 4200
//#define STABILIZE_ROLL_D 0.11
//#define STABILIZE_PITCH_D 0.11
// experimental!! // experimental!!
// Yaw is controled by targeting home. you will not have Yaw override. // Yaw is controled by targeting home. you will not have Yaw override.
// flying too close to home may induce spins. // flying too close to home may induce spins.
#define SIMPLE_LOOK_AT_HOME 0 #define SIMPLE_LOOK_AT_HOME 0
#define DYNAMIC_DRIFT 0 // careful!!! 0 = off, 1 = on

380
ArduCopterMega/ArduCopterMega.pde
View File

@ -251,6 +251,11 @@ const char* flight_mode_strings[] = {
8 TBD 8 TBD
*/ */
// test
//Vector3f accels_rot;
//float accel_gain = 20;
// Radio // Radio
// ----- // -----
byte control_mode = STABILIZE; byte control_mode = STABILIZE;
@ -304,6 +309,8 @@ int climb_rate; // m/s * 100 - For future implementation of controlled a
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
int last_ground_speed; // used to dampen navigation int last_ground_speed; // used to dampen navigation
int waypoint_speed;
byte wp_control; // used to control - navgation or loiter byte wp_control; // used to control - navgation or loiter
byte command_must_index; // current command memory location byte command_must_index; // current command memory location
@ -319,8 +326,6 @@ float sin_pitch_y, sin_yaw_y, sin_roll_y;
bool simple_bearing_is_set = false; bool simple_bearing_is_set = false;
long initial_simple_bearing; // used for Simple mode long initial_simple_bearing; // used for Simple mode
float Y6_scaling = Y6_MOTOR_SCALER;
// Airspeed // Airspeed
// -------- // --------
int airspeed; // m/s * 100 int airspeed; // m/s * 100
@ -334,6 +339,7 @@ long distance_error; // distance to the WP
long yaw_error; // how off are we pointed long yaw_error; // how off are we pointed
long long_error, lat_error; // temp for debugging long long_error, lat_error; // temp for debugging
int loiter_error_max; int loiter_error_max;
// Battery Sensors // Battery Sensors
// --------------- // ---------------
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
@ -350,11 +356,11 @@ float current_total;
// Barometer Sensor variables // Barometer Sensor variables
// -------------------------- // --------------------------
long abs_pressure; long abs_pressure;
long ground_pressure; long ground_pressure;
int ground_temperature; int ground_temperature;
int32_t baro_filter[BARO_FILTER_SIZE]; int32_t baro_filter[BARO_FILTER_SIZE];
byte baro_filter_index; byte baro_filter_index;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
@ -363,6 +369,7 @@ int baro_alt;
int baro_alt_offset; int baro_alt_offset;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific // flight mode specific
// -------------------- // --------------------
boolean takeoff_complete; // Flag for using take-off controls boolean takeoff_complete; // Flag for using take-off controls
@ -372,7 +379,7 @@ int landing_distance; // meters;
long old_alt; // used for managing altitude rates long old_alt; // used for managing altitude rates
int velocity_land; int velocity_land;
byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
byte brake_timer; int throttle_slew; // used to smooth throttle tranistions
// Loiter management // Loiter management
// ----------------- // -----------------
@ -388,11 +395,9 @@ long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs long nav_lat; // for error calcs
long nav_lon; // for error calcs long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // for filtering
long throttle_integrator; // used to control when we calculate nav_throttle long throttle_integrator; // used to control when we calculate nav_throttle
bool invalid_throttle; // used to control when we calculate nav_throttle bool invalid_throttle; // used to control when we calculate nav_throttle
bool invalid_nav; // used to control when we calculate nav_throttle
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
long command_yaw_start; // what angle were we to begin with long command_yaw_start; // what angle were we to begin with
@ -535,7 +540,6 @@ void loop()
medium_loop(); medium_loop();
// Stuff to run at full 50hz, but after the loops // Stuff to run at full 50hz, but after the loops
fifty_hz_loop(); fifty_hz_loop();
@ -563,7 +567,7 @@ void loop()
} }
// PORTK |= B01000000; // PORTK |= B01000000;
// PORTK &= B10111111; // PORTK &= B10111111;
//
// Main loop // Main loop
void fast_loop() void fast_loop()
{ {
@ -589,7 +593,7 @@ void fast_loop()
// record throttle output // record throttle output
// ------------------------------ // ------------------------------
//throttle_integrator += g.rc_3.servo_out; throttle_integrator += g.rc_3.servo_out;
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values // HIL for a copter needs very fast update of the servo values
@ -746,9 +750,6 @@ void medium_loop()
// ----------------------- // -----------------------
arm_motors(); arm_motors();
// should we be in DCM Fast recovery?
// ----------------------------------
check_DCM();
slow_loop(); slow_loop();
break; break;
@ -769,8 +770,10 @@ void fifty_hz_loop()
// use Yaw to find our bearing error // use Yaw to find our bearing error
calc_bearing_error(); calc_bearing_error();
// guess how close we are - fixed observer calc if (throttle_slew < 0)
//calc_distance_error(); throttle_slew++;
else if (throttle_slew > 0)
throttle_slew--;
# if HIL_MODE == HIL_MODE_DISABLED # if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
@ -1008,22 +1011,23 @@ void update_current_flight_mode(void)
switch(command_must_ID){ switch(command_must_ID){
default: default:
// Output Pitch, Roll, Yaw and Throttle // mix in user control with Nav control
// ------------------------------------ g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
auto_yaw(); g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
// mix in user control // Roll control
control_nav_mixer(); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
// perform stabilzation // Pitch control
output_stabilize_roll(); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
output_stabilize_pitch();
if(invalid_throttle) // Throttle control
calc_nav_throttle(); if(invalid_throttle){
auto_throttle();
}
// apply throttle control // Yaw control
output_auto_throttle(); g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break; break;
} }
@ -1031,58 +1035,37 @@ void update_current_flight_mode(void)
switch(control_mode){ switch(control_mode){
case ACRO: case ACRO:
// clear any AP naviagtion values // Roll control
nav_pitch = 0; g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
nav_roll = 0;
// 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 // Yaw control
output_manual_yaw(); g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in);
// apply throttle control
output_manual_throttle();
// mix in user control
control_nav_mixer();
// perform rate or stabilzation
// ----------------------------
// Roll control
if(abs(g.rc_1.control_in) >= ACRO_RATE_TRIGGER){
output_rate_roll(); // rate control yaw
}else{
output_stabilize_roll(); // hold yaw
}
// Roll control
if(abs(g.rc_2.control_in) >= ACRO_RATE_TRIGGER){
output_rate_pitch(); // rate control yaw
}else{
output_stabilize_pitch(); // hold yaw
}
break; break;
case STABILIZE: case STABILIZE:
// clear any AP naviagtion values // calcualte new nav_yaw offset
nav_pitch = 0; nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
nav_roll = 0;
if(g.rc_3.control_in == 0){ // Roll control
clear_yaw_control(); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
}else{
// Yaw control
output_manual_yaw();
}
// apply throttle control // Pitch control
output_manual_throttle(); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
// mix in user control // Throttle control
control_nav_mixer(); g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// perform stabilzation // Yaw control
output_stabilize_roll(); g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
output_stabilize_pitch();
//Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out);
break; break;
case SIMPLE: case SIMPLE:
@ -1091,11 +1074,12 @@ void update_current_flight_mode(void)
if(flight_timer > 4){ if(flight_timer > 4){
flight_timer = 0; flight_timer = 0;
// make sure this is always 0
simple_WP.lat = 0; simple_WP.lat = 0;
simple_WP.lng = 0; simple_WP.lng = 0;
next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres 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 next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres
//next_WP.lng = g.rc_1.control_in; // X: 4500 * .7 = 2250 = 25 meteres //next_WP.lng = g.rc_1.control_in; // X: 4500 * .7 = 2250 = 25 meteres
//next_WP.lat = -g.rc_2.control_in; // Y: 4500 * .7 = 2250 = 25 meteres //next_WP.lat = -g.rc_2.control_in; // Y: 4500 * .7 = 2250 = 25 meteres
@ -1103,6 +1087,7 @@ void update_current_flight_mode(void)
nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing;
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
wp_distance = get_distance(&simple_WP, &next_WP); wp_distance = get_distance(&simple_WP, &next_WP);
calc_bearing_error(); calc_bearing_error();
/* /*
@ -1117,10 +1102,9 @@ void update_current_flight_mode(void)
// get nav_pitch and nav_roll // get nav_pitch and nav_roll
calc_simple_nav(); calc_simple_nav();
calc_nav_output(); calc_nav_output();
limit_nav_pitch_roll(4500);
} }
/*
#if SIMPLE_LOOK_AT_HOME == 0 #if SIMPLE_LOOK_AT_HOME == 0
// This is typical yaw behavior // This is typical yaw behavior
@ -1141,96 +1125,98 @@ void update_current_flight_mode(void)
// ------------------------------------ // ------------------------------------
auto_yaw(); auto_yaw();
#endif #endif
*/
// apply throttle control // calcualte new nav_yaw offset
output_manual_throttle(); nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
// apply nav_pitch and nav_roll to output // Roll control
simple_mixer(); 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);
//Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew);
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
break; break;
case ALT_HOLD: case ALT_HOLD:
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// allow interactive changing of atitude // allow interactive changing of atitude
adjust_altitude(); adjust_altitude();
// 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
if(invalid_throttle){
auto_throttle();
}
// Yaw control // Yaw control
// ----------- g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
output_manual_yaw();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
// mix in user control
control_nav_mixer();
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
break; break;
case GUIDED: case GUIDED:
case RTL: case RTL:
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
// mix in user control with Nav control // mix in user control with Nav control
control_nav_mixer(); g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
// perform stabilzation // Roll control
output_stabilize_roll(); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out);
output_stabilize_pitch();
// 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);
break; break;
case LOITER: case LOITER:
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
// allow interactive changing of atitude // allow interactive changing of atitude
adjust_altitude(); adjust_altitude();
// Output Pitch, Roll, Yaw and Throttle // 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 // Yaw control
// ----------- g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
output_manual_yaw();
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
// mix in user control with Nav control
control_nav_mixer();
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
break; break;
default: default:
//Serial.print("$"); //Serial.print("$");
break; break;
} }
} }
} }
@ -1313,6 +1299,9 @@ 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;
//Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered();
} }
// updated at 10hz // updated at 10hz
@ -1333,9 +1322,9 @@ void update_alt()
int temp_sonar = sonar.read(); int temp_sonar = sonar.read();
// spike filter // spike filter
//if((temp_sonar - sonar_alt) < 50){ if((temp_sonar - sonar_alt) < 50){
// sonar_alt = temp_sonar; sonar_alt = temp_sonar;
//} }
sonar_alt = temp_sonar; sonar_alt = temp_sonar;
@ -1346,7 +1335,7 @@ void update_alt()
sonar_alt = (float)sonar_alt * temp; sonar_alt = (float)sonar_alt * temp;
*/ */
if(baro_alt < 800){ if(baro_alt < 1500){
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); scale = constrain(scale, 0, 1);
@ -1373,68 +1362,66 @@ adjust_altitude()
flight_timer = 0; flight_timer = 0;
if(g.rc_3.control_in <= 200){ if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second next_WP.alt -= 3; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 100)); // don't go more than 4 meters below current location next_WP.alt = max(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); // no lower than 1 meter?
}else if (g.rc_3.control_in > 700){ }else if (g.rc_3.control_in > 700){
next_WP.alt += 2; // 1 meter per second 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((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 + 200)); // 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
} }
} }
void tuning(){ void tuning(){
#if CHANNEL_6_TUNING == CH6_STABLIZE_KP //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_roll.kP((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
g.pid_stabilize_pitch.kD((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_roll.kD((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_BARO_KP #elif CHANNEL_6_TUNING == CH6_YAW_KP
g.pid_baro_throttle.kP((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_BARO_KD #elif CHANNEL_6_TUNING == CH6_YAW_KI
g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 g.pid_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_SONAR_KP
g.pid_sonar_throttle.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_SONAR_KD //Inner Loop : Rate
g.pid_sonar_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 #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);
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING #elif CHANNEL_6_TUNING == CH6_RATE_KI
Y6_scaling = (float)g.rc_6.control_in / 1000.0; 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);
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
g.pid_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);
//Altitude Hold
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
g.pid_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
//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_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
#elif CHANNEL_6_TUNING == CH6_DCM_RP
dcm.kp_roll_pitch((float)g.rc_6.control_in / 5000.0);
#elif CHANNEL_6_TUNING == CH6_DCM_Y
dcm.kp_yaw((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_KP
// yaw heading
g.pid_yaw.kP((float)g.rc_6.control_in / 200.0); // range from 0.0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_YAW_KD
// yaw heading
g.pid_yaw.kD((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
// yaw rate
g.pid_acro_rate_yaw.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
// yaw rate
g.pid_acro_rate_yaw.kD((float)g.rc_6.control_in / 1000.0);
#endif #endif
} }
@ -1477,20 +1464,19 @@ void point_at_home_yaw()
nav_yaw = get_bearing(&current_loc, &home); nav_yaw = get_bearing(&current_loc, &home);
} }
void check_DCM() void auto_throttle()
{ {
#if DYNAMIC_DRIFT == 1 // get the AP throttle
if(g.rc_1.control_in == 0 && g.rc_2.control_in == 0){ nav_throttle = get_nav_throttle(altitude_error);
if(g.rc_3.control_in < (g.throttle_cruise + 20)){
//dcm.kp_roll_pitch(dcm.kDCM_kp_rp_high); // apply throttle control
dcm.kp_roll_pitch(0.15); g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew);
}
}else{ // remember throttle offset
dcm.kp_roll_pitch(0.05967); throttle_slew = g.rc_3.servo_out - g.rc_3.control_in;
}
#endif // 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);
} }

376
ArduCopterMega/Attitude.pde
View File

@ -1,98 +1,118 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void // XXX TODO: convert these PI rate controlers to a Class
control_nav_mixer() int
get_stabilize_roll(long target_angle)
{ {
// control +- 45° is mixed with the navigation request by the Autopilot long error;
// output is in degrees = target pitch and roll of copter long rate;
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
}
void error = wrap_180(target_angle - dcm.roll_sensor);
simple_mixer()
{
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
g.rc_1.servo_out = nav_roll;
g.rc_2.servo_out = nav_pitch;
}
void
limit_nav_pitch_roll(long pmax)
{
// limit the nav pitch and roll of the copter
//long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
}
void
output_stabilize_roll()
{
float error;
error = g.rc_1.servo_out - dcm.roll_sensor;
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel // desired Rate:
g.rc_1.servo_out = g.pid_stabilize_roll.get_pi(error, delta_ms_fast_loop, 1.0); // 2500 * .7 = 1750 rate = g.pid_stabilize_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
// We adjust the output by the rate of rotation: // Rate P:
// Rate control through bias corrected gyro rates error = rate - (long)(degrees(omega.x) * 100.0);
// omega is the raw gyro reading rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
g.rc_1.servo_out -= degrees(omega.x) * 100.0 * g.pid_stabilize_roll.kD(); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
g.rc_1.servo_out = min(g.rc_1.servo_out, 2500);
g.rc_1.servo_out = max(g.rc_1.servo_out, -2500); // output control:
return (int)constrain(rate, -2500, 2500);
} }
void int
output_stabilize_pitch() get_stabilize_pitch(long target_angle)
{ {
float error; long error;
long rate;
error = g.rc_2.servo_out - dcm.pitch_sensor; error = wrap_180(target_angle - dcm.pitch_sensor);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel // desired Rate:
g.rc_2.servo_out = g.pid_stabilize_pitch.get_pi(error, delta_ms_fast_loop, 1.0); rate = g.pid_stabilize_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
// We adjust the output by the rate of rotation: // Rate P:
// Rate control through bias corrected gyro rates error = rate - (long)(degrees(omega.y) * 100.0);
// omega is the raw gyro reading rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0);
g.rc_2.servo_out -= degrees(omega.y) * 100.0 * g.pid_stabilize_pitch.kD(); //Serial.printf("%d\t%d\n", (int)error, (int)rate);
g.rc_2.servo_out = min(g.rc_2.servo_out, 2500);
g.rc_2.servo_out = max(g.rc_2.servo_out, -2500); // output control:
return (int)constrain(rate, -2500, 2500);
} }
void int
output_rate_roll() get_stabilize_yaw(long target_angle)
{ {
// rate control long error;
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377 long rate;
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700 error = wrap_180(target_angle - dcm.yaw_sensor);
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
// 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, 1.0);
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
// 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);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
// output control:
return (int)constrain(rate, -2500, 2500);
} }
void int
output_rate_pitch() get_rate_roll(long target_rate)
{ {
// rate control long error;
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377 target_rate = constrain(target_rate, -2500, 2500);
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700 error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400 target_rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
// output control:
return (int)constrain(target_rate, -2500, 2500);
} }
int
get_rate_pitch(long target_rate)
{
long error;
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);
// output control:
return (int)constrain(target_rate, -2500, 2500);
}
int
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);
// output control:
return (int)constrain(target_rate, -2500, 2500);
}
// 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
void void
@ -108,71 +128,45 @@ throttle control
// user input: // user input:
// ----------- // -----------
void output_manual_throttle() int
get_throttle(int throttle_input)
{ {
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost(); throttle_input = (float)throttle_input * angle_boost();
g.rc_3.servo_out = max(g.rc_3.servo_out, 0); throttle_input += throttle_slew;
return max(throttle_input, 0);
} }
// Autopilot long
// --------- get_nav_yaw_offset(int yaw_input, int throttle_input)
void output_auto_throttle()
{ {
g.rc_3.servo_out = (float)nav_throttle * angle_boost(); long _yaw;
// make sure we never send a 0 throttle that will cut the motors
g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
}
void calc_nav_throttle() if(throttle_input == 0){
{ // we are on the ground
// limit error return dcm.yaw_sensor;
nav_throttle = g.pid_baro_throttle.get_pid(altitude_error, delta_ms_medium_loop, 1.0);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 60);
// simple filtering
if(nav_throttle_old == 0)
nav_throttle_old = nav_throttle;
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
// clear the new data flag
invalid_throttle = false;
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
}
void calc_nav_throttle2()
{
// limit error
long error = constrain(altitude_error, -400, 400);
float scaler = 1.0;
if(error < 0){
// try and prevent rapid fall
scaler = (altitude_sensor == BARO) ? .8 : .8;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .2
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
}else{ }else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .5 // re-define nav_yaw if we have stick input
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -40, 100); if(yaw_input != 0){
// set nav_yaw + or - the current location
_yaw = (long)yaw_input + dcm.yaw_sensor;
// we need to wrap our value so we can be 0 to 360 (*100)
return wrap_360(_yaw);
}else{
return nav_yaw;
}
} }
// simple filtering
if(nav_throttle_old == 0)
nav_throttle_old = nav_throttle;
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
// clear the new data flag
invalid_throttle = false;
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
} }
/*
int alt_hold_velocity()
{
// subtract filtered Accel
float error = abs(next_WP.alt - current_loc.alt);
error = min(error, 200);
error = 1 - (error/ 200.0);
return (accels_rot.z + 9.81) * accel_gain * error;
}*/
float angle_boost() float angle_boost()
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
@ -180,137 +174,3 @@ float angle_boost()
return temp; return temp;
} }
/*************************************************************
yaw control
****************************************************************/
void output_manual_yaw()
{
// Yaw control
if(g.rc_4.control_in == 0){
output_yaw_with_hold(true); // hold yaw
}else{
output_yaw_with_hold(false); // rate control yaw
}
}
void auto_yaw()
{
output_yaw_with_hold(true); // hold yaw
}
void
clear_yaw_control()
{
//Serial.print("Clear ");
rate_yaw_flag = false; // exit rate_yaw_flag
nav_yaw = dcm.yaw_sensor; // save our Yaw
g.rc_4.servo_out = 0; // reset our output. It can stick when we are at 0 throttle
yaw_error = 0;
yaw_debug = YAW_HOLD; //0
}
#if YAW_OPTION == 0
void
output_yaw_with_hold(boolean hold)
{
// rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
if(hold){
// look to see if we have exited rate control properly - ie stopped turning
if(rate_yaw_flag){
// we are still in motion from rate control
if((fabs(omega.z) < .25) || (brake_timer < 2)){
clear_yaw_control();
hold = true; // just to be explicit
}else{
hold = false; // return to rate control until we slow down.
}
}
}else{
// rate control
// this indicates we are under rate control, when we enter Yaw Hold and
// return to 0° per second, we exit rate control and hold the current Yaw
rate_yaw_flag = true;
yaw_error = 0;
}
if(hold){
brake_timer = 0;
// try and hold the current nav_yaw setting
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
// we need to wrap our value so we can be -180 to 180 (*100)
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -9000, 9000); // limit error to 40 degees
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
// add in yaw dampener
g.rc_4.servo_out -= rate * g.pid_yaw.kD();
yaw_debug = YAW_HOLD; //0
}else{
if(g.rc_4.control_in == 0){
brake_timer--;
// adaptive braking
g.rc_4.servo_out = (int)(-1200.0 * omega.z);
yaw_debug = YAW_BRAKE; // 1
}else{
// RATE control
brake_timer = 100;
yaw_debug = YAW_RATE; // 2
long error = ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100); // control is += 4500 * 6 = 36000
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
}
}
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -3200, 3200); // limit to 32°
//Serial.printf("%d\n",g.rc_4.servo_out);
}
#elif YAW_OPTION == 1
void
output_yaw_with_hold(boolean hold)
{
// re-define nav_yaw if we have stick input
if(g.rc_4.control_in != 0){
// set nav_yaw + or - the current location
nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor;
//nav_yaw += (long)(g.rc_4.control_in / 90);
}
// we need to wrap our value so we can be 0 to 360 (*100)
nav_yaw = wrap_360(nav_yaw);
// how far off is nav_yaw from our current yaw?
yaw_error = nav_yaw - dcm.yaw_sensor;
// we need to wrap our value so we can be -180 to 180 (*100)
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -4500, 4500); // limit error to 60 degees
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
// add in yaw dampener
g.rc_4.servo_out -= (degrees(omega.z) * 100) * g.pid_yaw.kD();
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit error to 60 degees
}
#endif

39
ArduCopterMega/Log.pde
View File

@ -400,8 +400,12 @@ void Log_Write_Raw()
{ {
Vector3f gyro = imu.get_gyro(); Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
//Vector3f accel_filt = imu.get_accel_filtered();
gyro *= t7; // Scale up for storage as long integers gyro *= t7; // Scale up for storage as long integers
accel *= t7; accel *= t7;
//accel_filt *= t7;
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG); DataFlash.WriteByte(LOG_RAW_MSG);
@ -409,6 +413,12 @@ void Log_Write_Raw()
DataFlash.WriteLong((long)gyro.x); DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y); DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z); DataFlash.WriteLong((long)gyro.z);
//DataFlash.WriteLong((long)(accels_rot.x * t7));
//DataFlash.WriteLong((long)(accels_rot.y * t7));
//DataFlash.WriteLong((long)(accels_rot.z * t7));
DataFlash.WriteLong((long)accel.x); DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y); DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z); DataFlash.WriteLong((long)accel.z);
@ -417,6 +427,19 @@ void Log_Write_Raw()
} }
#endif #endif
// Read a raw accel/gyro packet
void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW,"));
for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
void Log_Write_Current() void Log_Write_Current()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
@ -534,7 +557,7 @@ 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_baro_throttle.get_integrator()); DataFlash.WriteInt((int)g.pid_throttle.get_integrator());
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -710,20 +733,6 @@ void Log_Read_Mode()
Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt()); Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
} }
// Read a raw accel/gyro packet
void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW,"));
for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
void Log_Write_Startup() void Log_Write_Startup()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);

531
ArduCopterMega/Parameters.h
View File

@ -9,223 +9,225 @@
// //
class Parameters { class Parameters {
public: public:
// The version of the layout as described by the parameter enum. // The version of the layout as described by the parameter enum.
// //
// When changing the parameter enum in an incompatible fashion, this // When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one. // value should be incremented by one.
// //
// 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 = 102; static const uint16_t k_format_version = 103;
// 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)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use // GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches. // values within that range to identify different branches.
// //
static const uint16_t k_software_type = 10; // 0 for APM trunk static const uint16_t k_software_type = 10; // 0 for APM trunk
// //
// Parameter identities. // Parameter identities.
// //
// The enumeration defined here is used to ensure that every parameter // The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by // or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM. // AP_Var to store and locate parameters in EEPROM.
// //
// Note that entries without a number are assigned the next number after // Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they // the entry preceding them. When adding new entries, ensure that they
// don't overlap. // don't overlap.
// //
// Try to group related variables together, and assign them a set // Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order // range in the enumeration. Place these groups in numerical order
// at the end of the enumeration. // at the end of the enumeration.
// //
// WARNING: Care should be taken when editing this enumeration as the // WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify // AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM. // variables saved in EEPROM.
// //
// //
enum { enum {
// Layout version number, always key zero. // Layout version number, always key zero.
// //
k_param_format_version = 0, k_param_format_version = 0,
k_param_software_type, k_param_software_type,
// Misc // Misc
// //
k_param_log_bitmask, k_param_log_bitmask,
// 110: Telemetry control // 110: Telemetry control
// //
k_param_streamrates_port0 = 110, k_param_streamrates_port0 = 110,
k_param_streamrates_port3, k_param_streamrates_port3,
k_param_sysid_this_mav, k_param_sysid_this_mav,
k_param_sysid_my_gcs, k_param_sysid_my_gcs,
// //
// 140: Sensor parameters // 140: Sensor parameters
// //
k_param_IMU_calibration = 140, k_param_IMU_calibration = 140,
k_param_battery_monitoring, k_param_battery_monitoring,
k_param_pack_capacity, k_param_pack_capacity,
k_param_compass_enabled, k_param_compass_enabled,
k_param_compass, k_param_compass,
k_param_sonar, k_param_sonar,
k_param_frame_orientation, k_param_frame_orientation,
k_param_top_bottom_ratio,
// //
// 160: Navigation parameters // 160: Navigation parameters
// //
k_param_crosstrack_gain = 160, k_param_crosstrack_entry_angle = 160,
k_param_crosstrack_entry_angle, k_param_pitch_max,
k_param_pitch_max, k_param_RTL_altitude,
k_param_RTL_altitude,
// //
// 180: Radio settings // 180: Radio settings
// //
k_param_rc_1 = 180, k_param_rc_1 = 180,
k_param_rc_2, k_param_rc_2,
k_param_rc_3, k_param_rc_3,
k_param_rc_4, k_param_rc_4,
k_param_rc_5, k_param_rc_5,
k_param_rc_6, k_param_rc_6,
k_param_rc_7, k_param_rc_7,
k_param_rc_8, k_param_rc_8,
k_param_rc_9, k_param_rc_9,
k_param_rc_10, k_param_rc_10,
k_param_throttle_min, k_param_throttle_min,
k_param_throttle_max, k_param_throttle_max,
k_param_throttle_fs_enabled, k_param_throttle_fs_enabled,
k_param_throttle_fs_action, k_param_throttle_fs_action,
k_param_throttle_fs_value, k_param_throttle_fs_value,
k_param_throttle_cruise, k_param_throttle_cruise,
k_param_flight_modes, k_param_flight_modes,
k_param_esc_calibrate, k_param_esc_calibrate,
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// //
// 200: Heli // 200: Heli
// //
k_param_heli_servo_1 = 200, k_param_heli_servo_1 = 200,
k_param_heli_servo_2, k_param_heli_servo_2,
k_param_heli_servo_3, k_param_heli_servo_3,
k_param_heli_servo_4, k_param_heli_servo_4,
k_param_heli_servo1_pos , k_param_heli_servo1_pos ,
k_param_heli_servo2_pos, k_param_heli_servo2_pos,
k_param_heli_servo3_pos, k_param_heli_servo3_pos,
k_param_heli_roll_max, k_param_heli_roll_max,
k_param_heli_pitch_max, k_param_heli_pitch_max,
k_param_heli_collective_min, k_param_heli_collective_min,
k_param_heli_collective_max, k_param_heli_collective_max,
k_param_heli_collective_mid, k_param_heli_collective_mid,
k_param_heli_ext_gyro_enabled, k_param_heli_ext_gyro_enabled,
k_param_heli_ext_gyro_gain, // 213 k_param_heli_ext_gyro_gain, // 213
#endif #endif
// //
// 220: Waypoint data // 220: Waypoint data
// //
k_param_waypoint_mode = 220, k_param_waypoint_mode = 220,
k_param_waypoint_total, k_param_waypoint_total,
k_param_waypoint_index, k_param_waypoint_index,
k_param_command_must_index, k_param_command_must_index,
k_param_waypoint_radius, k_param_waypoint_radius,
k_param_loiter_radius, k_param_loiter_radius,
k_param_waypoint_speed_max,
// //
// 240: PID Controllers // 240: PID Controllers
// //
// Heading-to-roll PID: // Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim // heading error from commnd to roll command deviation from trim
// (bank to turn strategy) // (bank to turn strategy)
// //
k_param_pid_acro_rate_roll = 240, k_param_pid_rate_roll = 240,
k_param_pid_acro_rate_pitch, k_param_pid_rate_pitch,
k_param_pid_acro_rate_yaw, k_param_pid_rate_yaw,
k_param_pid_stabilize_roll, k_param_pid_stabilize_roll,
k_param_pid_stabilize_pitch, k_param_pid_stabilize_pitch,
k_param_pid_yaw, k_param_pid_stabilize_yaw,
k_param_pid_nav_lat, k_param_pid_nav_lat,
k_param_pid_nav_lon, k_param_pid_nav_lon,
k_param_pid_nav_wp, k_param_pid_nav_wp,
k_param_pid_baro_throttle, k_param_pid_throttle,
k_param_pid_sonar_throttle, k_param_pid_crosstrack,
// 255: reserved // 255: reserved
}; };
AP_Int16 format_version; AP_Int16 format_version;
AP_Int8 software_type; AP_Int8 software_type;
// Telemetry control // Telemetry control
// //
AP_Int16 sysid_this_mav; AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
// Crosstrack navigation // Crosstrack navigation
// //
AP_Float crosstrack_gain; AP_Int16 crosstrack_entry_angle;
AP_Int16 crosstrack_entry_angle;
// Waypoints // Waypoints
// //
AP_Int8 waypoint_mode; AP_Int8 waypoint_mode;
AP_Int8 waypoint_total; AP_Int8 waypoint_total;
AP_Int8 waypoint_index; AP_Int8 waypoint_index;
AP_Int8 command_must_index; AP_Int8 command_must_index;
AP_Int8 waypoint_radius; AP_Int8 waypoint_radius;
AP_Int8 loiter_radius; AP_Int8 loiter_radius;
AP_Int16 waypoint_speed_max;
// Throttle // Throttle
// //
AP_Int16 throttle_min; AP_Int16 throttle_min;
AP_Int16 throttle_max; AP_Int16 throttle_max;
AP_Int8 throttle_fs_enabled; AP_Int8 throttle_fs_enabled;
AP_Int8 throttle_fs_action; AP_Int8 throttle_fs_action;
AP_Int16 throttle_fs_value; AP_Int16 throttle_fs_value;
AP_Int16 throttle_cruise; AP_Int16 throttle_cruise;
// Flight modes // Flight modes
// //
AP_VarA<uint8_t,6> flight_modes; AP_VarA<uint8_t,6> flight_modes;
// Radio settings // Radio settings
// //
//AP_Var_group pwm_roll; //AP_Var_group pwm_roll;
//AP_Var_group pwm_pitch; //AP_Var_group pwm_pitch;
//AP_Var_group pwm_throttle; //AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw; //AP_Var_group pwm_yaw;
AP_Int16 pitch_max; AP_Int16 pitch_max;
// Misc // Misc
// //
AP_Int16 log_bitmask; AP_Int16 log_bitmask;
AP_Int16 RTL_altitude; AP_Int16 RTL_altitude;
AP_Int8 sonar_enabled; AP_Int8 sonar_enabled;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled; AP_Int8 compass_enabled;
AP_Int8 esc_calibrate; AP_Int8 esc_calibrate;
AP_Int8 frame_orientation; AP_Int8 frame_orientation;
AP_Float top_bottom_ratio;
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// Heli // Heli
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo) AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
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;
@ -237,112 +239,115 @@ public:
RC_Channel rc_camera_pitch; RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll; RC_Channel rc_camera_roll;
// PID controllers // PID controllers
PID pid_acro_rate_roll; PID pid_rate_roll;
PID pid_acro_rate_pitch; PID pid_rate_pitch;
PID pid_acro_rate_yaw; PID pid_rate_yaw;
PID pid_stabilize_roll; PID pid_stabilize_roll;
PID pid_stabilize_pitch; PID pid_stabilize_pitch;
PID pid_yaw; PID pid_stabilize_yaw;
PID pid_nav_lat; PID pid_nav_lat;
PID pid_nav_lon; PID pid_nav_lon;
PID pid_nav_wp; PID pid_nav_wp;
PID pid_baro_throttle; PID pid_throttle;
PID pid_sonar_throttle; PID pid_crosstrack;
uint8_t junk; uint8_t junk;
// Note: keep initializers here in the same order as they are declared above. // Note: keep initializers here in the same order as they are declared above.
Parameters() : Parameters() :
// variable default key name // variable default key name
//------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")), format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")), software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")), sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")), sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), //crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")), crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")), sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")), throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")),
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")), throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")),
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
flight_modes (k_param_flight_modes, PSTR("FLTMODE")), flight_modes (k_param_flight_modes, PSTR("FLTMODE")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")),
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")), heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")), heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")), heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")), heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
heli_servo1_pos (-60, k_param_heli_servo1_pos, PSTR("SV1_POS_")), heli_servo1_pos (-60, k_param_heli_servo1_pos, PSTR("SV1_POS_")),
heli_servo2_pos (60, k_param_heli_servo2_pos, PSTR("SV2_POS_")), heli_servo2_pos (60, k_param_heli_servo2_pos, PSTR("SV2_POS_")),
heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")), heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
heli_roll_max (4500, k_param_heli_roll_max, PSTR("ROL_MAX_")), heli_roll_max (4500, k_param_heli_roll_max, PSTR("ROL_MAX_")),
heli_pitch_max (4500, k_param_heli_pitch_max, PSTR("PIT_MAX_")), heli_pitch_max (4500, k_param_heli_pitch_max, PSTR("PIT_MAX_")),
heli_coll_min (1000, k_param_heli_collective_min, PSTR("COL_MIN_")), heli_coll_min (1000, k_param_heli_collective_min, PSTR("COL_MIN_")),
heli_coll_max (2000, k_param_heli_collective_max, PSTR("COL_MAX_")), heli_coll_max (2000, k_param_heli_collective_max, PSTR("COL_MAX_")),
heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")), heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")),
heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")), heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")),
heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")), heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
#endif #endif
// RC channel group key name // RC channel group key name
//---------------------------------------------------------------------- //----------------------------------------------------------------------
rc_1 (k_param_rc_1, PSTR("RC1_")), rc_1 (k_param_rc_1, PSTR("RC1_")),
rc_2 (k_param_rc_2, PSTR("RC2_")), rc_2 (k_param_rc_2, PSTR("RC2_")),
rc_3 (k_param_rc_3, PSTR("RC3_")), rc_3 (k_param_rc_3, PSTR("RC3_")),
rc_4 (k_param_rc_4, PSTR("RC4_")), rc_4 (k_param_rc_4, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")), rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")), rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")), rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")), rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, NULL), rc_camera_pitch (k_param_rc_9, NULL),
rc_camera_roll (k_param_rc_10, NULL), rc_camera_roll (k_param_rc_10, NULL),
// PID controller group key name initial P initial I initial D initial imax // PID controller group key name initial P initial I initial D initial imax
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pid_acro_rate_roll (k_param_pid_acro_rate_roll, PSTR("ACR_RLL_"), ACRO_RATE_ROLL_P, ACRO_RATE_ROLL_I, ACRO_RATE_ROLL_D, ACRO_RATE_ROLL_IMAX * 100), pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, 0, RATE_ROLL_IMAX * 100),
pid_acro_rate_pitch (k_param_pid_acro_rate_pitch, PSTR("ACR_PIT_"), ACRO_RATE_PITCH_P, ACRO_RATE_PITCH_I, ACRO_RATE_PITCH_D, ACRO_RATE_PITCH_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_acro_rate_yaw (k_param_pid_acro_rate_yaw, PSTR("ACR_YAW_"), ACRO_RATE_YAW_P, ACRO_RATE_YAW_I, ACRO_RATE_YAW_D, ACRO_RATE_YAW_IMAX * 100), pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, 0, RATE_YAW_IMAX * 100),
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_D, STABILIZE_ROLL_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, STABILIZE_PITCH_D, STABILIZE_PITCH_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_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, YAW_D, YAW_IMAX * 100), pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, 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_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_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), 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_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX), pid_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX), pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, 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
} {
}
}; };
#endif // PARAMETERS_H #endif // PARAMETERS_H

2
ArduCopterMega/commands.pde
View File

@ -207,8 +207,6 @@ void set_next_WP(struct Location *wp)
// ------------------------------- // -------------------------------
void init_home() void init_home()
{ {
SendDebugln("MSG: <init_home> init home");
// block until we get a good fix // block until we get a good fix
// ----------------------------- // -----------------------------
while (!g_gps->new_data || !g_gps->fix) { while (!g_gps->new_data || !g_gps->fix) {

199
ArduCopterMega/config.h
View File

@ -50,8 +50,8 @@
// Sonar // Sonar
// //
#ifndef SONAR_PIN #ifndef SONAR_PORT
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE # define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE
#endif #endif
#ifndef SONAR_TYPE #ifndef SONAR_TYPE
@ -274,119 +274,79 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Y6 Support // Y6 Support
#ifndef Y6_MOTOR_SCALER #ifndef TOP_BOTTOM_RATIO
# define Y6_MOTOR_SCALER 0.92 # define TOP_BOTTOM_RATIO 0.92
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// ACRO Rate Control
#ifndef ACRO_RATE_ROLL_P
# define ACRO_RATE_ROLL_P .190
#endif
#ifndef ACRO_RATE_ROLL_I
# define ACRO_RATE_ROLL_I 0.0
#endif
#ifndef ACRO_RATE_ROLL_D
# define ACRO_RATE_ROLL_D 0.0
#endif
#ifndef ACRO_RATE_ROLL_IMAX
# define ACRO_RATE_ROLL_IMAX 20
#endif
#ifndef ACRO_RATE_PITCH_P
# define ACRO_RATE_PITCH_P .190
#endif
#ifndef ACRO_RATE_PITCH_I
# define ACRO_RATE_PITCH_I 0.0
#endif
#ifndef ACRO_RATE_PITCH_D
# define ACRO_RATE_PITCH_D 0.0
#endif
#ifndef ACRO_RATE_PITCH_IMAX
# define ACRO_RATE_PITCH_IMAX 20
#endif
#ifndef ACRO_RATE_YAW_P
# define ACRO_RATE_YAW_P .13 // used to control response in turning
#endif
#ifndef ACRO_RATE_YAW_I
# define ACRO_RATE_YAW_I 0.0
#endif
#ifndef ACRO_RATE_YAW_D
# define ACRO_RATE_YAW_D 0.00 //
#endif
#ifndef ACRO_RATE_YAW_IMAX
# define ACRO_RATE_YAW_IMAX 0
#endif
#ifndef ACRO_RATE_TRIGGER
# define ACRO_RATE_TRIGGER 0
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// STABILZE Angle Control // Roll Control
// //
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 0.48 // .48 = 4.0 NG, .54 = 4.5 NG # define STABILIZE_ROLL_P 4.5
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025 // # define STABILIZE_ROLL_I 0.025
#endif
#ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.18
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX .5 // degrees * 100 # define STABILIZE_ROLL_IMAX .5 // degrees
#endif #endif
#ifndef RATE_ROLL_P
# define RATE_ROLL_P .12
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.0
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 15 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Pitch Control
//
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.48 # define STABILIZE_PITCH_P 4.5
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.025 # define STABILIZE_PITCH_I 0.025
#endif #endif
#ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.18
#endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX .5 // degrees * 100 # define STABILIZE_PITCH_IMAX .5 // degrees
#endif
#ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.12
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.0
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 15 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// YAW Control // YAW Control
// //
#ifndef YAW_OPTION #ifndef STABILIZE_YAW_P
# define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach # define STABILIZE_YAW_P 4.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef STABILIZE_YAW_IMAX
# define STABILIZE_YAW_IMAX 15 // degrees * 100
#endif #endif
#if YAW_OPTION == 1 // 0 = hybrid rate approach, 1 = offset Yaw approach #ifndef RATE_YAW_P
#ifndef YAW_P # define RATE_YAW_P .15 // used to control response in turning
# define YAW_P 0.6 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif
#endif #ifndef RATE_YAW_I
#ifndef YAW_I # define RATE_YAW_I 0.0
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance #endif
#endif #ifndef RATE_YAW_IMAX
#ifndef YAW_D # define RATE_YAW_IMAX 50
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100
#endif
#else
#ifndef YAW_P
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef YAW_I
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef YAW_D
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100
#endif
#endif #endif
@ -395,7 +355,7 @@
// //
// how much to we pitch towards the target // how much to we pitch towards the target
#ifndef PITCH_MAX #ifndef PITCH_MAX
# define PITCH_MAX 16 // degrees # define PITCH_MAX 22 // degrees
#endif #endif
@ -403,16 +363,16 @@
// Navigation control gains // Navigation control gains
// //
#ifndef NAV_LOITER_P #ifndef NAV_LOITER_P
# define NAV_LOITER_P 1.4 // # define NAV_LOITER_P 1.3 //
#endif #endif
#ifndef NAV_LOITER_I #ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.1 // # define NAV_LOITER_I 0.01 //
#endif #endif
#ifndef NAV_LOITER_D #ifndef NAV_LOITER_D
# define NAV_LOITER_D 0.4 // # define NAV_LOITER_D 0.4 //
#endif #endif
#ifndef NAV_LOITER_IMAX #ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 15 // degrees° # define NAV_LOITER_IMAX 10 // degrees°
#endif #endif
@ -431,51 +391,48 @@
#endif #endif
#ifndef WAYPOINT_SPEED #ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED 600 // for 6m/s error = 13mph # define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_BARO_P #ifndef THROTTLE_P
# define THROTTLE_BARO_P 0.3 // trying a lower val # define THROTTLE_P 0.4 // trying a lower val
#endif #endif
#ifndef THROTTLE_BARO_I #ifndef THROTTLE_I
# define THROTTLE_BARO_I 0.04 //with 4m error, 12.5s windup # define THROTTLE_I 0.01 //with 4m error, 12.5s windup
#endif #endif
#ifndef THROTTLE_BARO_D #ifndef THROTTLE_D
# define THROTTLE_BARO_D 0.35 // upped with filter # define THROTTLE_D 0.35 // upped with filter
#endif #endif
#ifndef THROTTLE_BARO_IMAX #ifndef THROTTLE_IMAX
# define THROTTLE_BARO_IMAX 30 # define THROTTLE_IMAX 30
#endif
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P 0.35 // lowering P by .15
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.05
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.3 // increasing D by .5
#endif
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 30
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation // Crosstrack compensation
// //
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 2 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE #ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 20 // deg # define XTRACK_ENTRY_ANGLE 30 // deg
#endif #endif
#ifndef XTRACK_P
# define XTRACK_P 2 // trying a lower val
#endif
#ifndef XTRACK_I
# define XTRACK_I 0.00 //with 4m error, 12.5s windup
#endif
#ifndef XTRACK_D
# define XTRACK_D 0.00 // upped with filter
#endif
#ifndef XTRACK_IMAX
# define XTRACK_IMAX 10
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////

4
ArduCopterMega/control_modes.pde
View File

@ -33,8 +33,8 @@ void reset_control_switch()
{ {
oldSwitchPosition = -1; oldSwitchPosition = -1;
read_control_switch(); read_control_switch();
SendDebug("MSG: reset_control_switch "); //SendDebug("MSG: reset_control_switch ");
SendDebugln(oldSwitchPosition , DEC); //SendDebugln(oldSwitchPosition , DEC);
} }
void update_servo_switches() void update_servo_switches()

34
ArduCopterMega/defines.h
View File

@ -21,6 +21,7 @@
#define Y6_FRAME 3 #define Y6_FRAME 3
#define OCTA_FRAME 4 #define OCTA_FRAME 4
#define HELI_FRAME 5 #define HELI_FRAME 5
#define OCTA_QUAD_FRAME 6
#define PLUS_FRAME 0 #define PLUS_FRAME 0
#define X_FRAME 1 #define X_FRAME 1
@ -126,23 +127,27 @@
#define YAW_BRAKE 1 #define YAW_BRAKE 1
#define YAW_RATE 2 #define YAW_RATE 2
// CH_6 Tuning // CH_6 Tuning
// ----------- // -----------
#define CH6_NONE 0 #define CH6_NONE 0
#define CH6_STABLIZE_KP 1 // Attitude
#define CH6_STABLIZE_KD 2 #define CH6_STABILIZE_KP 1
#define CH6_BARO_KP 3 #define CH6_STABILIZE_KI 2
#define CH6_BARO_KD 4 #define CH6_YAW_KP 3
#define CH6_SONAR_KP 5 #define CH6_YAW_KD 4
#define CH6_SONAR_KD 6 // Rate
#define CH6_Y6_SCALING 7 #define CH6_RATE_KP 5
#define CH6_PMAX 8 #define CH6_RATE_KI 6
#define CH6_DCM_RP 9 #define CH6_YAW_RATE_KP 7
#define CH6_DCM_Y 10 #define CH6_YAW_RATE_KD 8
#define CH6_YAW_KP 11 // Altitude
#define CH6_YAW_KD 12 #define CH6_THROTTLE_KP 9
#define CH6_YAW_RATE_KP 13 #define CH6_THROTTLE_KD 10
#define CH6_YAW_RATE_KD 14 // Extras
#define CH6_TOP_BOTTOM_RATIO 11
#define CH6_PMAX 12
// nav byte mask // nav byte mask
// ------------- // -------------
@ -340,7 +345,6 @@
#define PIEZO_PIN AN5 //Last pin on the back ADC connector #define PIEZO_PIN AN5 //Last pin on the back ADC connector
#define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL
// sonar // sonar
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters

11
ArduCopterMega/leds.pde
View File

@ -99,12 +99,19 @@ void update_motor_leds(void)
if (blink){ if (blink){
blink = false; blink = false;
digitalWrite(RE_LED, LOW); digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{ }else{
blink = true; blink = true;
digitalWrite(RE_LED, HIGH); digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
} }
} }
#endif #endif

12
ArduCopterMega/motors.pde
View File

@ -28,9 +28,19 @@ void arm_motors()
arming_counter = ARM_DELAY; arming_counter = ARM_DELAY;
// Remember Orientation // Remember Orientation
// --------------------------- // --------------------
init_simple_bearing(); init_simple_bearing();
// Reset home position
// ----------------------
if(home_is_set)
init_home();
// tune down compass
// -----------------
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
arming_counter++; arming_counter++;
} else{ } else{
arming_counter++; arming_counter++;

159
ArduCopterMega/motors_octa_quad.pde Normal file
View File

@ -0,0 +1,159 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == OCTA_QUAD_FRAME
void output_motors_armed()
{
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = (float)g.rc_1.pwm_out * .707;
pitch_out = (float)g.rc_2.pwm_out * .707;
// Front Left
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW
// Front Right
motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW
// Back Left
motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW
// Back Right
motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP
motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW
}if(g.frame_orientation == PLUS_FRAME){
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
// Left
motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP
motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW
// Right
motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP
motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW
// Front
motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP
motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW
// Back
motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW
}
// Yaw
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_10] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
motor_out[CH_11] -= g.rc_4.pwm_out; // CW
// limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min);
motor_out[CH_10] = max(motor_out[CH_10], out_min);
motor_out[CH_11] = max(motor_out[CH_11], out_min);
#if CUT_MOTORS == ENABLED
// Send commands to motors
if(g.rc_3.servo_out > 0){
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.OutputCh(CH_10, motor_out[CH_10]);
APM_RC.OutputCh(CH_11, motor_out[CH_11]);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out6_Out7();
APM_RC.Force_Out2_Out3();
}else{
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
}
#else
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.OutputCh(CH_10, motor_out[CH_10]);
APM_RC.OutputCh(CH_11, motor_out[CH_11]);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out6_Out7();
APM_RC.Force_Out2_Out3();
#endif
}
void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 11; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
}
void output_motor_test()
{
}
#endif

6
ArduCopterMega/motors_y6.pde
View File

@ -21,15 +21,15 @@ void output_motors_armed()
int pitch_out = g.rc_2.pwm_out / 2; int pitch_out = g.rc_2.pwm_out / 2;
//left //left
motor_out[CH_2] = ((g.rc_3.radio_out * Y6_scaling) + roll_out + pitch_out); // CCW TOP motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right //right
motor_out[CH_7] = ((g.rc_3.radio_out * Y6_scaling) - roll_out + pitch_out); // CCW TOP motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back //back
motor_out[CH_8] = ((g.rc_3.radio_out * Y6_scaling) - g.rc_2.pwm_out); // CCW TOP motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
// Yaw // Yaw

40
ArduCopterMega/navigation.pde
View File

@ -43,6 +43,20 @@ bool check_missed_wp()
return (abs(temp) > 10000); //we pased the waypoint by 10 ° return (abs(temp) > 10000); //we pased the waypoint by 10 °
} }
int
get_nav_throttle(long error)
{
int throttle;
// limit error
throttle = g.pid_throttle.get_pid(error, delta_ms_medium_loop, 1.0);
throttle = g.throttle_cruise + constrain(throttle, -80, 80);
//int tem = alt_hold_velocity();
//throttle -= tem;
return throttle;
}
#define DIST_ERROR_MAX 1800 #define DIST_ERROR_MAX 1800
void calc_loiter_nav() void calc_loiter_nav()
{ {
@ -68,10 +82,6 @@ void calc_loiter_nav()
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750, 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 = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
//long pmax = g.pitch_max.get();
//nav_lon = constrain(nav_lon, -pmax, pmax);
//nav_lat = constrain(nav_lat, -pmax, pmax);
} }
void calc_loiter_output() void calc_loiter_output()
@ -102,10 +112,6 @@ void calc_loiter_output()
//WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward //WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back //EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
//limit our copter pitch - this will change if we go to a fully rate limited approach.
//limit_nav_pitch_roll(g.pitch_max.get());
} }
void calc_simple_nav() void calc_simple_nav()
@ -137,16 +143,19 @@ void calc_rate_nav()
// calc the cos of the error to tell how fast we are moving towards the target in cm // calc the cos of the error to tell how fast we are moving towards the target in cm
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100)); int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
// change to rate error // Reduce speed on RTL
// we want to be going 450cm/s if(control_mode == RTL){
int error = constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000); waypoint_speed = min((wp_distance * 100), 600);
waypoint_speed = max(g.waypoint_speed_max.get(), 30);
}else{
waypoint_speed = g.waypoint_speed_max.get();
}
int error = constrain(waypoint_speed - groundspeed, -1000, 1000);
// Scale response by kP // Scale response by kP
nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0); nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0);
nav_lat >>= 1; // divide by two nav_lat >>= 1; // divide by two
// unfiltered:
//nav_lat = g.pid_nav_wp.get_pid(error, dTnav, 1.0);
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
// limit our output // limit our output
@ -199,7 +208,8 @@ void update_crosstrack(void)
// ---------------- // ----------------
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
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); 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); nav_bearing = wrap_360(nav_bearing);
} }
} }

38
ArduCopterMega/radio.pde
View File

@ -19,15 +19,10 @@ void init_rc_in()
g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
// set rc dead zones // set rc dead zones
g.rc_1.dead_zone = 0; // 60 = .6 degrees g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 0; g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 60; g.rc_3.dead_zone = 60;
g.rc_4.dead_zone = 500;// 0 = hybrid rate approach
#if YAW_OPTION == 1
g.rc_4.dead_zone = 500;// 1 = offset Yaw approach
#else
g.rc_4.dead_zone = 800;// 0 = hybrid rate approach
#endif
//set auxiliary ranges //set auxiliary ranges
g.rc_5.set_range(0,1000); g.rc_5.set_range(0,1000);
@ -36,20 +31,27 @@ void init_rc_in()
g.rc_7.set_range(0,1000); g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);
#if CHANNEL_6_TUNING == CH6_STABLIZE_KD #if CHANNEL_6_TUNING == CH6_RATE_KP
g.rc_6.set_range(0,300); g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_BARO_KP #elif CHANNEL_6_TUNING == CH6_RATE_KI
g.rc_6.set_range(0,800); g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_BARO_KD #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
g.rc_6.set_range(0,500); g.rc_6.set_range(0,8000); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
g.rc_6.set_range(800,1000); g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
g.rc_6.set_range(0,800); // 0 to .8
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
g.rc_6.set_range(0,500); // 0 to .5
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
g.rc_6.set_range(800,1000); // .8 to 1
#endif #endif
//catch bad RC_3 min values
} }
void init_rc_out() void init_rc_out()

44
ArduCopterMega/setup.pde
View File

@ -11,7 +11,7 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
@ -36,7 +36,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"battery", setup_batt_monitor}, {"battery", setup_batt_monitor},
{"sonar", setup_sonar}, {"sonar", setup_sonar},
{"compass", setup_compass}, {"compass", setup_compass},
{"offsets", setup_mag_offset}, // {"offsets", setup_mag_offset},
{"declination", setup_declination}, {"declination", setup_declination},
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
{"heli", setup_heli}, {"heli", setup_heli},
@ -84,9 +84,9 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_frame(); report_frame();
report_batt_monitor(); report_batt_monitor();
report_sonar(); report_sonar();
report_gains(); //report_gains();
report_xtrack(); //report_xtrack();
report_throttle(); //report_throttle();
report_flight_modes(); report_flight_modes();
report_imu(); report_imu();
report_compass(); report_compass();
@ -660,7 +660,7 @@ void clear_offsets()
compass.save_offsets(); compass.save_offsets();
} }
static int8_t /*static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv) setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{ {
Vector3f _offsets; Vector3f _offsets;
@ -720,7 +720,9 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
} }
return 0;
} }
*/
/***************************************************************************/ /***************************************************************************/
@ -853,18 +855,19 @@ void report_radio()
print_blanks(2); print_blanks(2);
} }
/*
void report_gains() void report_gains()
{ {
Serial.printf_P(PSTR("Gains\n")); Serial.printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
// Acro // Rate
Serial.printf_P(PSTR("Acro:\nroll:\n")); Serial.printf_P(PSTR("Rate:\nroll:\n"));
print_PID(&g.pid_acro_rate_roll); print_PID(&g.pid_rate_roll);
Serial.printf_P(PSTR("pitch:\n")); Serial.printf_P(PSTR("pitch:\n"));
print_PID(&g.pid_acro_rate_pitch); print_PID(&g.pid_rate_pitch);
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_acro_rate_yaw); print_PID(&g.pid_rate_yaw);
// Stabilize // Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n")); Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
@ -872,7 +875,7 @@ void report_gains()
Serial.printf_P(PSTR("pitch:\n")); Serial.printf_P(PSTR("pitch:\n"));
print_PID(&g.pid_stabilize_pitch); print_PID(&g.pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_yaw); print_PID(&g.pid_stabilize_yaw);
//Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener); //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); //Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
@ -882,14 +885,13 @@ void report_gains()
print_PID(&g.pid_nav_lat); print_PID(&g.pid_nav_lat);
Serial.printf_P(PSTR("long:\n")); Serial.printf_P(PSTR("long:\n"));
print_PID(&g.pid_nav_lon); print_PID(&g.pid_nav_lon);
Serial.printf_P(PSTR("baro throttle:\n")); Serial.printf_P(PSTR("throttle:\n"));
print_PID(&g.pid_baro_throttle); print_PID(&g.pid_throttle);
Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&g.pid_sonar_throttle);
print_blanks(2); print_blanks(2);
} }
*/
void report_xtrack() /*void report_xtrack()
{ {
Serial.printf_P(PSTR("XTrack\n")); Serial.printf_P(PSTR("XTrack\n"));
print_divider(); print_divider();
@ -902,8 +904,9 @@ void report_xtrack()
(long)g.pitch_max); (long)g.pitch_max);
print_blanks(2); print_blanks(2);
} }
*/
void report_throttle() /*void report_throttle()
{ {
Serial.printf_P(PSTR("Throttle\n")); Serial.printf_P(PSTR("Throttle\n"));
print_divider(); print_divider();
@ -919,7 +922,7 @@ void report_throttle()
(int)g.throttle_fs_enabled, (int)g.throttle_fs_enabled,
(int)g.throttle_fs_value); (int)g.throttle_fs_value);
print_blanks(2); print_blanks(2);
} }*/
void report_imu() void report_imu()
{ {
@ -1002,7 +1005,7 @@ void report_gyro()
// CLI utilities // CLI utilities
/***************************************************************************/ /***************************************************************************/
void /*void
print_PID(PID * pid) print_PID(PID * 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, D:%4.2f, IMAX:%ld\n"),
@ -1011,6 +1014,7 @@ print_PID(PID * pid)
pid->kD(), pid->kD(),
(long)pid->imax()); (long)pid->imax());
} }
*/
void void
print_radio_values() print_radio_values()

32
ArduCopterMega/system.pde
View File

@ -40,7 +40,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.33 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.34 Beta", main_menu_commands);
void init_ardupilot() void init_ardupilot()
{ {
@ -208,14 +208,10 @@ void init_ardupilot()
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){ if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc); sonar.init(SONAR_PORT, &adc);
} }
#endif #endif
// setup DCM for copters:
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate
// Logging: // Logging:
// -------- // --------
// DataFlash log initialization // DataFlash log initialization
@ -256,23 +252,15 @@ void init_ardupilot()
start_new_log(); start_new_log();
} }
//if (g.log_bitmask & MASK_LOG_MODE)
// Log_Write_Mode(control_mode);
// All of the Gyro calibrations // All of the Gyro calibrations
// ---------------------------- // ----------------------------
startup_ground(); startup_ground();
// set the correct flight mode // set the correct flight mode
// --------------------------- // ---------------------------
//reset_control_switch(); reset_control_switch();
// init the Yaw Hold output
clear_yaw_control();
delay(100); delay(100);
} }
//******************************************************************************** //********************************************************************************
@ -313,6 +301,7 @@ void startup_ground(void)
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();
@ -326,10 +315,20 @@ void startup_ground(void)
break; break;
} }
} }
//*/
// setup DCM for copters:
dcm.kp_roll_pitch(0.12); // higher for quads
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
clear_leds(); clear_leds();
// print the GPS status
report_gps(); report_gps();
// lenthen the idle timeout for gps Auto_detect
g_gps->idleTimeout = 20000; g_gps->idleTimeout = 20000;
// used to limit the input of error for loiter // used to limit the input of error for loiter
@ -376,8 +375,7 @@ void set_mode(byte mode)
case SIMPLE: case SIMPLE:
case STABILIZE: case STABILIZE:
do_loiter_at_location(); do_loiter_at_location();
g.pid_baro_throttle.reset_I(); g.pid_throttle.reset_I();
g.pid_sonar_throttle.reset_I();
break; break;
case ALT_HOLD: case ALT_HOLD:

84
ArduCopterMega/test.pde
View File

@ -423,6 +423,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
{ {
//Serial.printf_P(PSTR("Calibrating.")); //Serial.printf_P(PSTR("Calibrating."));
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
report_imu(); report_imu();
imu.init_gyro(); imu.init_gyro();
report_imu(); report_imu();
@ -446,8 +449,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f accels = imu.get_accel(); Vector3f accels = imu.get_accel();
Vector3f gyros = imu.get_gyro(); Vector3f gyros = imu.get_gyro();
//Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * accel_filt;
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 4){ if(medium_loopCounter == 4){
update_trig(); update_trig();
} }
@ -473,7 +480,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled){ if(g.compass_enabled){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading compass.calculate(dcm.get_dcm_matrix());
} }
} }
@ -494,6 +501,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
cos_yaw_x, // x cos_yaw_x, // x
sin_yaw_y); // y sin_yaw_y); // y
//*/ //*/
//
Log_Write_Raw();
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -675,51 +685,51 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
delay(200); delay(200);
read_radio(); read_radio();
//Outer Loop : Attitude
#if CHANNEL_6_TUNING == CH6_NONE #if CHANNEL_6_TUNING == CH6_NONE
Serial.printf_P(PSTR("disabled\n")); Serial.printf_P(PSTR("disabled\n"));
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KP #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
Serial.printf_P(PSTR("stab kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_BARO_KP
Serial.printf_P(PSTR("baro kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_BARO_KD
Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_SONAR_KP
Serial.printf_P(PSTR("sonar kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_SONAR_KD
Serial.printf_P(PSTR("sonar kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_DCM_RP
Serial.printf_P(PSTR("DCM RP: %1.4f\n"), ((float)g.rc_6.control_in / 5000.0));
#elif CHANNEL_6_TUNING == CH6_DCM_Y
Serial.printf_P(PSTR("DCM Y: %1.4f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_KP #elif CHANNEL_6_TUNING == CH6_YAW_KP
// yaw heading Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0
Serial.printf_P(PSTR("yaw kP: %1.3f\n"), ((float)g.rc_6.control_in / 200.0)); // range from 0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_YAW_KD #elif CHANNEL_6_TUNING == CH6_YAW_KI
// yaw heading Serial.printf_P(PSTR("yaw Hold kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
Serial.printf_P(PSTR("yaw kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Inner Loop : Rate
#elif CHANNEL_6_TUNING == CH6_RATE_KP
Serial.printf_P(PSTR("rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_RATE_KI
Serial.printf_P(PSTR("rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
// yaw rate
Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI
// yaw rate Serial.printf_P(PSTR("yaw rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
Serial.printf_P(PSTR("yaw rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Altitude Hold
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KP
Serial.printf_P(PSTR("throttle kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_THROTTLE_KD
Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
//Extras
#elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO
Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_PMAX
Serial.printf_P(PSTR("Y6: %d\n"), (g.rc_6.control_in * 2));
#endif #endif
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -817,7 +827,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) {
return (0); return (0);
} }
} }
} }
static int8_t static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv) test_xbee(uint8_t argc, const Menu::arg *argv)
@ -866,9 +876,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
print_hit_enter(); print_hit_enter();
while(1){ while(1){
delay(250); delay(100);
compass.read(); compass.read();
compass.calculate(0,0); compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets(); Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100, (wrap_360(ToDeg(compass.heading) * 100)) /100,