Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
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! 

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
/*
CH6_NONE
CH6_STABLIZE_KP
CH6_STABLIZE_KD
CH6_BARO_KP
CH6_BARO_KD
CH6_SONAR_KP
CH6_SONAR_KD
CH6_Y6_SCALING
CH6_STABILIZE_KP
CH6_STABILIZE_KI
CH6_RATE_KP
CH6_RATE_KI
CH6_THROTTLE_KP
CH6_THROTTLE_KD
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!!
// Yaw is controled by targeting home. you will not have Yaw override.
// flying too close to home may induce spins.
#define SIMPLE_LOOK_AT_HOME 0
#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
*/
// test
//Vector3f accels_rot;
//float accel_gain = 20;
// Radio
// -----
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?
int last_ground_speed; // used to dampen navigation
int waypoint_speed;
byte wp_control; // used to control - navgation or loiter
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;
long initial_simple_bearing; // used for Simple mode
float Y6_scaling = Y6_MOTOR_SCALER;
// Airspeed
// --------
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 long_error, lat_error; // temp for debugging
int loiter_error_max;
// Battery Sensors
// ---------------
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
// --------------------------
long abs_pressure;
long ground_pressure;
int ground_temperature;
int32_t baro_filter[BARO_FILTER_SIZE];
byte baro_filter_index;
long abs_pressure;
long ground_pressure;
int ground_temperature;
int32_t baro_filter[BARO_FILTER_SIZE];
byte baro_filter_index;
// Altitude Sensor variables
// ----------------------
@ -363,6 +369,7 @@ int baro_alt;
int baro_alt_offset;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific
// --------------------
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
int velocity_land;
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
// -----------------
@ -388,11 +395,9 @@ long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs
long nav_lon; // for error calcs
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_nav; // used to control when we calculate nav_throttle
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
long command_yaw_start; // what angle were we to begin with
@ -535,7 +540,6 @@ void loop()
medium_loop();
// Stuff to run at full 50hz, but after the loops
fifty_hz_loop();
@ -563,7 +567,7 @@ void loop()
}
// PORTK |= B01000000;
// PORTK &= B10111111;
//
// Main loop
void fast_loop()
{
@ -589,7 +593,7 @@ void fast_loop()
// 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
// HIL for a copter needs very fast update of the servo values
@ -746,9 +750,6 @@ void medium_loop()
// -----------------------
arm_motors();
// should we be in DCM Fast recovery?
// ----------------------------------
check_DCM();
slow_loop();
break;
@ -769,8 +770,10 @@ void fifty_hz_loop()
// use Yaw to find our bearing error
calc_bearing_error();
// guess how close we are - fixed observer calc
//calc_distance_error();
if (throttle_slew < 0)
throttle_slew++;
else if (throttle_slew > 0)
throttle_slew--;
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
@ -1008,22 +1011,23 @@ void update_current_flight_mode(void)
switch(command_must_ID){
default:
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
// 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);
// mix in user control
control_nav_mixer();
// Roll control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
if(invalid_throttle)
calc_nav_throttle();
// Throttle control
if(invalid_throttle){
auto_throttle();
}
// apply throttle control
output_auto_throttle();
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break;
}
@ -1031,58 +1035,37 @@ void update_current_flight_mode(void)
switch(control_mode){
case ACRO:
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// Roll control
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
// Pitch control
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// Yaw control
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;
case STABILIZE:
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
if(g.rc_3.control_in == 0){
clear_yaw_control();
}else{
// Yaw control
output_manual_yaw();
}
// Roll control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
// apply throttle control
output_manual_throttle();
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
// mix in user control
control_nav_mixer();
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
//Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out);
break;
case SIMPLE:
@ -1091,11 +1074,12 @@ void update_current_flight_mode(void)
if(flight_timer > 4){
flight_timer = 0;
// make sure this is always 0
simple_WP.lat = 0;
simple_WP.lng = 0;
next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres
next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres
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.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 = wrap_360(nav_bearing);
wp_distance = get_distance(&simple_WP, &next_WP);
calc_bearing_error();
/*
@ -1117,10 +1102,9 @@ void update_current_flight_mode(void)
// get nav_pitch and nav_roll
calc_simple_nav();
calc_nav_output();
limit_nav_pitch_roll(4500);
}
/*
#if SIMPLE_LOOK_AT_HOME == 0
// This is typical yaw behavior
@ -1141,96 +1125,98 @@ void update_current_flight_mode(void)
// ------------------------------------
auto_yaw();
#endif
*/
// apply throttle control
output_manual_throttle();
// calcualte new nav_yaw offset
nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in);
// apply nav_pitch and nav_roll to output
simple_mixer();
// Roll control
g.rc_1.servo_out = get_stabilize_roll(nav_roll);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(nav_pitch);
// Throttle control
g.rc_3.servo_out = get_throttle(g.rc_3.control_in);
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
//Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew);
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
break;
case ALT_HOLD:
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// allow interactive changing of atitude
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
// -----------
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();
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break;
case GUIDED:
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
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
output_stabilize_roll();
output_stabilize_pitch();
// Roll control
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out);
// Pitch control
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out);
// Throttle control
if(invalid_throttle){
auto_throttle();
}
// Yaw control
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break;
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
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
// -----------
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();
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
break;
default:
//Serial.print("$");
break;
}
}
}
@ -1313,6 +1299,9 @@ void update_trig(void){
cos_roll_x = temp.c.z / 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
@ -1333,9 +1322,9 @@ void update_alt()
int temp_sonar = sonar.read();
// spike filter
//if((temp_sonar - sonar_alt) < 50){
// sonar_alt = temp_sonar;
//}
if((temp_sonar - sonar_alt) < 50){
sonar_alt = temp_sonar;
}
sonar_alt = temp_sonar;
@ -1346,7 +1335,7 @@ void update_alt()
sonar_alt = (float)sonar_alt * temp;
*/
if(baro_alt < 800){
if(baro_alt < 1500){
scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1);
@ -1373,68 +1362,66 @@ adjust_altitude()
flight_timer = 0;
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 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, 100); // no lower than 1 meter?
next_WP.alt -= 3; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 600)); // don't go more than 4 meters below current location
}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(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(){
#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_pitch.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD
g.pid_stabilize_pitch.kD((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_roll.kD((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
g.pid_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_BARO_KP
g.pid_baro_throttle.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_YAW_KP
g.pid_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_BARO_KD
g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1
#elif CHANNEL_6_TUNING == CH6_YAW_KI
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
g.pid_sonar_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1
//Inner Loop : Rate
#elif CHANNEL_6_TUNING == CH6_RATE_KP
g.pid_rate_roll.kP((float)g.rc_6.control_in / 1000.0);
g.pid_rate_pitch.kP((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
Y6_scaling = (float)g.rc_6.control_in / 1000.0;
#elif CHANNEL_6_TUNING == CH6_RATE_KI
g.pid_rate_roll.kI((float)g.rc_6.control_in / 1000.0);
g.pid_rate_pitch.kI((float)g.rc_6.control_in / 1000.0);
#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
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
}
@ -1477,20 +1464,19 @@ void point_at_home_yaw()
nav_yaw = get_bearing(&current_loc, &home);
}
void check_DCM()
void auto_throttle()
{
#if DYNAMIC_DRIFT == 1
if(g.rc_1.control_in == 0 && g.rc_2.control_in == 0){
if(g.rc_3.control_in < (g.throttle_cruise + 20)){
//dcm.kp_roll_pitch(dcm.kDCM_kp_rp_high);
dcm.kp_roll_pitch(0.15);
}
}else{
dcm.kp_roll_pitch(0.05967);
}
#endif
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error);
// apply throttle control
g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew);
// remember throttle offset
throttle_slew = g.rc_3.servo_out - g.rc_3.control_in;
// 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 -*-
void
control_nav_mixer()
// XXX TODO: convert these PI rate controlers to a Class
int
get_stabilize_roll(long target_angle)
{
// 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 = g.rc_1.control_mix(nav_roll);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
}
long error;
long rate;
void
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;
error = wrap_180(target_angle - dcm.roll_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel
g.rc_1.servo_out = g.pid_stabilize_roll.get_pi(error, delta_ms_fast_loop, 1.0); // 2500 * .7 = 1750
// desired Rate:
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 control through bias corrected gyro rates
// omega is the raw gyro reading
g.rc_1.servo_out -= degrees(omega.x) * 100.0 * g.pid_stabilize_roll.kD();
g.rc_1.servo_out = min(g.rc_1.servo_out, 2500);
g.rc_1.servo_out = max(g.rc_1.servo_out, -2500);
// Rate P:
error = rate - (long)(degrees(omega.x) * 100.0);
rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
// output control:
return (int)constrain(rate, -2500, 2500);
}
void
output_stabilize_pitch()
int
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
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
g.rc_2.servo_out = g.pid_stabilize_pitch.get_pi(error, delta_ms_fast_loop, 1.0);
// desired Rate:
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 control through bias corrected gyro rates
// omega is the raw gyro reading
g.rc_2.servo_out -= degrees(omega.y) * 100.0 * g.pid_stabilize_pitch.kD();
g.rc_2.servo_out = min(g.rc_2.servo_out, 2500);
g.rc_2.servo_out = max(g.rc_2.servo_out, -2500);
// Rate P:
error = rate - (long)(degrees(omega.y) * 100.0);
rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
// output control:
return (int)constrain(rate, -2500, 2500);
}
void
output_rate_roll()
int
get_stabilize_yaw(long target_angle)
{
// rate control
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
long error;
long rate;
g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
error = wrap_180(target_angle - dcm.yaw_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -4500, 4500);
// desired Rate:
rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, 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
output_rate_pitch()
int
get_rate_roll(long target_rate)
{
// rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
long error;
target_rate = constrain(target_rate, -2500, 2500);
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
target_rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0);
// 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.
// Keeps outdated data out of our calculations
void
@ -108,71 +128,45 @@ throttle control
// 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();
g.rc_3.servo_out = max(g.rc_3.servo_out, 0);
throttle_input = (float)throttle_input * angle_boost();
throttle_input += throttle_slew;
return max(throttle_input, 0);
}
// Autopilot
// ---------
void output_auto_throttle()
long
get_nav_yaw_offset(int yaw_input, int throttle_input)
{
g.rc_3.servo_out = (float)nav_throttle * angle_boost();
// 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);
}
long _yaw;
void calc_nav_throttle()
{
// limit error
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);
if(throttle_input == 0){
// we are on the ground
return dcm.yaw_sensor;
// 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{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -40, 100);
// re-define nav_yaw if we have stick input
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 temp = cos_pitch_x * cos_roll_x;
@ -180,137 +174,3 @@ float angle_boost()
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 accel = imu.get_accel();
//Vector3f accel_filt = imu.get_accel_filtered();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
//accel_filt *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
@ -409,6 +413,12 @@ void Log_Write_Raw()
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
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.y);
DataFlash.WriteLong((long)accel.z);
@ -417,6 +427,19 @@ void Log_Write_Raw()
}
#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()
{
DataFlash.WriteByte(HEAD_BYTE1);
@ -534,7 +557,7 @@ void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)next_WP.alt); //
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);
}
@ -710,20 +733,6 @@ void Log_Read_Mode()
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()
{
DataFlash.WriteByte(HEAD_BYTE1);

531
ArduCopterMega/Parameters.h
View File

@ -9,223 +9,225 @@
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 102;
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 103;
// The parameter software_type is set up solely for ground station use
// 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.
//
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.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
//
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
k_param_log_bitmask,
// Misc
//
k_param_log_bitmask,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
//
// 140: Sensor parameters
//
k_param_IMU_calibration = 140,
k_param_battery_monitoring,
k_param_pack_capacity,
k_param_compass_enabled,
k_param_compass,
k_param_sonar,
k_param_frame_orientation,
//
// 140: Sensor parameters
//
k_param_IMU_calibration = 140,
k_param_battery_monitoring,
k_param_pack_capacity,
k_param_compass_enabled,
k_param_compass,
k_param_sonar,
k_param_frame_orientation,
k_param_top_bottom_ratio,
//
// 160: Navigation parameters
//
k_param_crosstrack_gain = 160,
k_param_crosstrack_entry_angle,
k_param_pitch_max,
k_param_RTL_altitude,
//
// 160: Navigation parameters
//
k_param_crosstrack_entry_angle = 160,
k_param_pitch_max,
k_param_RTL_altitude,
//
// 180: Radio settings
//
k_param_rc_1 = 180,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_action,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_flight_modes,
k_param_esc_calibrate,
//
// 180: Radio settings
//
k_param_rc_1 = 180,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_action,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_flight_modes,
k_param_esc_calibrate,
#if FRAME_CONFIG == HELI_FRAME
//
// 200: Heli
//
k_param_heli_servo_1 = 200,
k_param_heli_servo_2,
k_param_heli_servo_3,
k_param_heli_servo_4,
k_param_heli_servo1_pos ,
k_param_heli_servo2_pos,
k_param_heli_servo3_pos,
k_param_heli_roll_max,
k_param_heli_pitch_max,
k_param_heli_collective_min,
k_param_heli_collective_max,
k_param_heli_collective_mid,
k_param_heli_ext_gyro_enabled,
k_param_heli_ext_gyro_gain, // 213
#endif
#if FRAME_CONFIG == HELI_FRAME
//
// 200: Heli
//
k_param_heli_servo_1 = 200,
k_param_heli_servo_2,
k_param_heli_servo_3,
k_param_heli_servo_4,
k_param_heli_servo1_pos ,
k_param_heli_servo2_pos,
k_param_heli_servo3_pos,
k_param_heli_roll_max,
k_param_heli_pitch_max,
k_param_heli_collective_min,
k_param_heli_collective_max,
k_param_heli_collective_mid,
k_param_heli_ext_gyro_enabled,
k_param_heli_ext_gyro_gain, // 213
#endif
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_command_must_index,
k_param_waypoint_radius,
k_param_loiter_radius,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_command_must_index,
k_param_waypoint_radius,
k_param_loiter_radius,
k_param_waypoint_speed_max,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pid_acro_rate_roll = 240,
k_param_pid_acro_rate_pitch,
k_param_pid_acro_rate_yaw,
k_param_pid_stabilize_roll,
k_param_pid_stabilize_pitch,
k_param_pid_yaw,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pid_nav_wp,
k_param_pid_baro_throttle,
k_param_pid_sonar_throttle,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pid_rate_roll = 240,
k_param_pid_rate_pitch,
k_param_pid_rate_yaw,
k_param_pid_stabilize_roll,
k_param_pid_stabilize_pitch,
k_param_pid_stabilize_yaw,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pid_nav_wp,
k_param_pid_throttle,
k_param_pid_crosstrack,
// 255: reserved
};
// 255: reserved
};
AP_Int16 format_version;
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Crosstrack navigation
//
AP_Int16 crosstrack_entry_angle;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 command_must_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 command_must_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
AP_Int16 waypoint_speed_max;
// Throttle
//
AP_Int16 throttle_min;
AP_Int16 throttle_max;
AP_Int8 throttle_fs_enabled;
AP_Int8 throttle_fs_action;
AP_Int16 throttle_fs_value;
AP_Int16 throttle_cruise;
// Throttle
//
AP_Int16 throttle_min;
AP_Int16 throttle_max;
AP_Int8 throttle_fs_enabled;
AP_Int8 throttle_fs_action;
AP_Int16 throttle_fs_value;
AP_Int16 throttle_cruise;
// Flight modes
//
AP_VarA<uint8_t,6> flight_modes;
// Flight modes
//
AP_VarA<uint8_t,6> flight_modes;
// Radio settings
//
//AP_Var_group pwm_roll;
//AP_Var_group pwm_pitch;
//AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw;
// Radio settings
//
//AP_Var_group pwm_roll;
//AP_Var_group pwm_pitch;
//AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw;
AP_Int16 pitch_max;
AP_Int16 pitch_max;
// Misc
//
AP_Int16 log_bitmask;
AP_Int16 RTL_altitude;
// Misc
//
AP_Int16 log_bitmask;
AP_Int16 RTL_altitude;
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 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_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled;
AP_Int8 compass_enabled;
AP_Int8 esc_calibrate;
AP_Int8 frame_orientation;
AP_Float top_bottom_ratio;
#if FRAME_CONFIG == HELI_FRAME
#if FRAME_CONFIG == HELI_FRAME
// Heli
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_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_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)
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_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_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)
#endif
// RC channels
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
@ -237,112 +239,115 @@ public:
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
// PID controllers
PID pid_acro_rate_roll;
PID pid_acro_rate_pitch;
PID pid_acro_rate_yaw;
// PID controllers
PID pid_rate_roll;
PID pid_rate_pitch;
PID pid_rate_yaw;
PID pid_stabilize_roll;
PID pid_stabilize_pitch;
PID pid_yaw;
PID pid_stabilize_yaw;
PID pid_nav_lat;
PID pid_nav_lon;
PID pid_nav_wp;
PID pid_baro_throttle;
PID pid_sonar_throttle;
PID pid_throttle;
PID pid_crosstrack;
uint8_t junk;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------------------
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")),
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------------------
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")),
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_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")),
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_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")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_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")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_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")),
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_max (1000, k_param_throttle_max, PSTR("THR_MAX")),
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_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
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_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_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")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
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")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
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
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
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_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
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_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_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_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
#endif
#if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
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_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
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_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_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_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
#endif
// RC channel group key name
//----------------------------------------------------------------------
rc_1 (k_param_rc_1, PSTR("RC1_")),
rc_2 (k_param_rc_2, PSTR("RC2_")),
rc_3 (k_param_rc_3, PSTR("RC3_")),
rc_4 (k_param_rc_4, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, NULL),
rc_camera_roll (k_param_rc_10, NULL),
// RC channel group key name
//----------------------------------------------------------------------
rc_1 (k_param_rc_1, PSTR("RC1_")),
rc_2 (k_param_rc_2, PSTR("RC2_")),
rc_3 (k_param_rc_3, PSTR("RC3_")),
rc_4 (k_param_rc_4, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, NULL),
rc_camera_roll (k_param_rc_10, NULL),
// 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_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_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 controller group key name initial P initial I initial D initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------
pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, 0, RATE_ROLL_IMAX * 100),
pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, 0, RATE_PITCH_IMAX * 100),
pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, 0, RATE_YAW_IMAX * 100),
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_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_D, STABILIZE_PITCH_IMAX * 100),
pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, YAW_D, YAW_IMAX * 100),
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100),
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100),
pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, STABILIZE_YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_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_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX),
pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, XTRACK_IMAX),
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

2
ArduCopterMega/commands.pde
View File

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

199
ArduCopterMega/config.h
View File

@ -50,8 +50,8 @@
// Sonar
//
#ifndef SONAR_PIN
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
#ifndef SONAR_PORT
# define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE
#endif
#ifndef SONAR_TYPE
@ -274,119 +274,79 @@
//////////////////////////////////////////////////////////////////////////////
// Y6 Support
#ifndef Y6_MOTOR_SCALER
# define Y6_MOTOR_SCALER 0.92
#ifndef TOP_BOTTOM_RATIO
# define TOP_BOTTOM_RATIO 0.92
#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
# define STABILIZE_ROLL_P 0.48 // .48 = 4.0 NG, .54 = 4.5 NG
# define STABILIZE_ROLL_P 4.5
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025 //
#endif
#ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.18
# define STABILIZE_ROLL_I 0.025
#endif
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX .5 // degrees * 100
# define STABILIZE_ROLL_IMAX .5 // degrees
#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
# define STABILIZE_PITCH_P 0.48
# define STABILIZE_PITCH_P 4.5
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.025
#endif
#ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.18
#endif
#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
//////////////////////////////////////////////////////////////////////////////
// YAW Control
//
#ifndef YAW_OPTION
# define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach
#ifndef STABILIZE_YAW_P
# 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
#if YAW_OPTION == 1 // 0 = hybrid rate approach, 1 = offset Yaw approach
#ifndef YAW_P
# define YAW_P 0.6 // 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
#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
#ifndef RATE_YAW_P
# define RATE_YAW_P .15 // used to control response in turning
#endif
#ifndef RATE_YAW_I
# define RATE_YAW_I 0.0
#endif
#ifndef RATE_YAW_IMAX
# define RATE_YAW_IMAX 50
#endif
@ -395,7 +355,7 @@
//
// how much to we pitch towards the target
#ifndef PITCH_MAX
# define PITCH_MAX 16 // degrees
# define PITCH_MAX 22 // degrees
#endif
@ -403,16 +363,16 @@
// Navigation control gains
//
#ifndef NAV_LOITER_P
# define NAV_LOITER_P 1.4 //
# define NAV_LOITER_P 1.3 //
#endif
#ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.1 //
# define NAV_LOITER_I 0.01 //
#endif
#ifndef NAV_LOITER_D
# define NAV_LOITER_D 0.4 //
#endif
#ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 15 // degrees°
# define NAV_LOITER_IMAX 10 // degrees°
#endif
@ -431,51 +391,48 @@
#endif
#ifndef WAYPOINT_SPEED
# define WAYPOINT_SPEED 600 // for 6m/s error = 13mph
#ifndef WAYPOINT_SPEED_MAX
# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THROTTLE_BARO_P
# define THROTTLE_BARO_P 0.3 // trying a lower val
#ifndef THROTTLE_P
# define THROTTLE_P 0.4 // trying a lower val
#endif
#ifndef THROTTLE_BARO_I
# define THROTTLE_BARO_I 0.04 //with 4m error, 12.5s windup
#ifndef THROTTLE_I
# define THROTTLE_I 0.01 //with 4m error, 12.5s windup
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.35 // upped with filter
#ifndef THROTTLE_D
# define THROTTLE_D 0.35 // upped with filter
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_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
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 30
#endif
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 2 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 20 // deg
# define XTRACK_ENTRY_ANGLE 30 // deg
#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;
read_control_switch();
SendDebug("MSG: reset_control_switch ");
SendDebugln(oldSwitchPosition , DEC);
//SendDebug("MSG: reset_control_switch ");
//SendDebugln(oldSwitchPosition , DEC);
}
void update_servo_switches()

34
ArduCopterMega/defines.h
View File

@ -21,6 +21,7 @@
#define Y6_FRAME 3
#define OCTA_FRAME 4
#define HELI_FRAME 5
#define OCTA_QUAD_FRAME 6
#define PLUS_FRAME 0
#define X_FRAME 1
@ -126,23 +127,27 @@
#define YAW_BRAKE 1
#define YAW_RATE 2
// CH_6 Tuning
// -----------
#define CH6_NONE 0
#define CH6_STABLIZE_KP 1
#define CH6_STABLIZE_KD 2
#define CH6_BARO_KP 3
#define CH6_BARO_KD 4
#define CH6_SONAR_KP 5
#define CH6_SONAR_KD 6
#define CH6_Y6_SCALING 7
#define CH6_PMAX 8
#define CH6_DCM_RP 9
#define CH6_DCM_Y 10
#define CH6_YAW_KP 11
#define CH6_YAW_KD 12
#define CH6_YAW_RATE_KP 13
#define CH6_YAW_RATE_KD 14
// Attitude
#define CH6_STABILIZE_KP 1
#define CH6_STABILIZE_KI 2
#define CH6_YAW_KP 3
#define CH6_YAW_KD 4
// Rate
#define CH6_RATE_KP 5
#define CH6_RATE_KI 6
#define CH6_YAW_RATE_KP 7
#define CH6_YAW_RATE_KD 8
// Altitude
#define CH6_THROTTLE_KP 9
#define CH6_THROTTLE_KD 10
// Extras
#define CH6_TOP_BOTTOM_RATIO 11
#define CH6_PMAX 12
// nav byte mask
// -------------
@ -340,7 +345,6 @@
#define PIEZO_PIN AN5 //Last pin on the back ADC connector
#define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL
// sonar
#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){
blink = false;
digitalWrite(RE_LED, LOW);
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
blink = true;
digitalWrite(RE_LED, HIGH);
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
}
#endif

12
ArduCopterMega/motors.pde
View File

@ -28,9 +28,19 @@ void arm_motors()
arming_counter = ARM_DELAY;
// Remember Orientation
// ---------------------------
// --------------------
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++;
} else{
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;
//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
//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
//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
// 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 °
}
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
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_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()
@ -102,10 +112,6 @@ void calc_loiter_output()
//WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//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()
@ -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
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
// change to rate error
// we want to be going 450cm/s
int error = constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000);
// Reduce speed on RTL
if(control_mode == RTL){
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
nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0);
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);
// 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
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);
}
}

38
ArduCopterMega/radio.pde
View File

@ -19,15 +19,10 @@ void init_rc_in()
g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
// set rc dead zones
g.rc_1.dead_zone = 0; // 60 = .6 degrees
g.rc_2.dead_zone = 0;
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 60;
#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
g.rc_4.dead_zone = 500;// 0 = hybrid rate approach
//set auxiliary ranges
g.rc_5.set_range(0,1000);
@ -36,20 +31,27 @@ void init_rc_in()
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
#if CHANNEL_6_TUNING == CH6_STABLIZE_KD
g.rc_6.set_range(0,300);
#if CHANNEL_6_TUNING == CH6_RATE_KP
g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_BARO_KP
g.rc_6.set_range(0,800);
#elif CHANNEL_6_TUNING == CH6_RATE_KI
g.rc_6.set_range(0,300); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_BARO_KD
g.rc_6.set_range(0,500);
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KP
g.rc_6.set_range(0,8000); // 0 t0 .3
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
g.rc_6.set_range(800,1000);
#elif CHANNEL_6_TUNING == CH6_STABILIZE_KI
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
//catch bad RC_3 min values
}
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_sonar (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_esc (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},
{"sonar", setup_sonar},
{"compass", setup_compass},
{"offsets", setup_mag_offset},
// {"offsets", setup_mag_offset},
{"declination", setup_declination},
#if FRAME_CONFIG == HELI_FRAME
{"heli", setup_heli},
@ -84,9 +84,9 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_frame();
report_batt_monitor();
report_sonar();
report_gains();
report_xtrack();
report_throttle();
//report_gains();
//report_xtrack();
//report_throttle();
report_flight_modes();
report_imu();
report_compass();
@ -660,7 +660,7 @@ void clear_offsets()
compass.save_offsets();
}
static int8_t
/*static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
Vector3f _offsets;
@ -720,7 +720,9 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
return 0;
}
}
return 0;
}
*/
/***************************************************************************/
@ -853,18 +855,19 @@ void report_radio()
print_blanks(2);
}
/*
void report_gains()
{
Serial.printf_P(PSTR("Gains\n"));
print_divider();
// Acro
Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&g.pid_acro_rate_roll);
// Rate
Serial.printf_P(PSTR("Rate:\nroll:\n"));
print_PID(&g.pid_rate_roll);
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"));
print_PID(&g.pid_acro_rate_yaw);
print_PID(&g.pid_rate_yaw);
// Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
@ -872,7 +875,7 @@ void report_gains()
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&g.pid_stabilize_pitch);
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("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
@ -882,14 +885,13 @@ void report_gains()
print_PID(&g.pid_nav_lat);
Serial.printf_P(PSTR("long:\n"));
print_PID(&g.pid_nav_lon);
Serial.printf_P(PSTR("baro throttle:\n"));
print_PID(&g.pid_baro_throttle);
Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&g.pid_sonar_throttle);
Serial.printf_P(PSTR("throttle:\n"));
print_PID(&g.pid_throttle);
print_blanks(2);
}
*/
void report_xtrack()
/*void report_xtrack()
{
Serial.printf_P(PSTR("XTrack\n"));
print_divider();
@ -902,8 +904,9 @@ void report_xtrack()
(long)g.pitch_max);
print_blanks(2);
}
*/
void report_throttle()
/*void report_throttle()
{
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
@ -919,7 +922,7 @@ void report_throttle()
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(2);
}
}*/
void report_imu()
{
@ -1002,7 +1005,7 @@ void report_gyro()
// CLI utilities
/***************************************************************************/
void
/*void
print_PID(PID * pid)
{
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(),
(long)pid->imax());
}
*/
void
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.
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()
{
@ -208,14 +208,10 @@ void init_ardupilot()
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
sonar.init(SONAR_PORT, &adc);
}
#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:
// --------
// DataFlash log initialization
@ -256,23 +252,15 @@ void init_ardupilot()
start_new_log();
}
//if (g.log_bitmask & MASK_LOG_MODE)
// Log_Write_Mode(control_mode);
// All of the Gyro calibrations
// ----------------------------
startup_ground();
// set the correct flight mode
// ---------------------------
//reset_control_switch();
// init the Yaw Hold output
clear_yaw_control();
reset_control_switch();
delay(100);
}
//********************************************************************************
@ -313,6 +301,7 @@ void startup_ground(void)
GPS_enabled = false;
//*
// Read in the GPS
for (byte counter = 0; ; counter++) {
g_gps->update();
@ -326,10 +315,20 @@ void startup_ground(void)
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();
// print the GPS status
report_gps();
// lenthen the idle timeout for gps Auto_detect
g_gps->idleTimeout = 20000;
// used to limit the input of error for loiter
@ -376,8 +375,7 @@ void set_mode(byte mode)
case SIMPLE:
case STABILIZE:
do_loiter_at_location();
g.pid_baro_throttle.reset_I();
g.pid_sonar_throttle.reset_I();
g.pid_throttle.reset_I();
break;
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."));
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
report_imu();
imu.init_gyro();
report_imu();
@ -446,8 +449,12 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f accels = imu.get_accel();
Vector3f gyros = imu.get_gyro();
//Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * accel_filt;
medium_loopCounter++;
if(medium_loopCounter == 4){
update_trig();
}
@ -473,7 +480,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled){
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
sin_yaw_y); // y
//*/
//
Log_Write_Raw();
}
if(Serial.available() > 0){
@ -675,51 +685,51 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
delay(200);
read_radio();
//Outer Loop : Attitude
#if CHANNEL_6_TUNING == CH6_NONE
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));
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD
Serial.printf_P(PSTR("stab kD: %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_STABILIZE_KI
Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_KP
// yaw heading
Serial.printf_P(PSTR("yaw kP: %1.3f\n"), ((float)g.rc_6.control_in / 200.0)); // range from 0 ~ 5.0
Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0
#elif CHANNEL_6_TUNING == CH6_YAW_KD
// yaw heading
Serial.printf_P(PSTR("yaw kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_KI
Serial.printf_P(PSTR("yaw Hold kI: %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
// yaw rate
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
// yaw rate
Serial.printf_P(PSTR("yaw rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI
Serial.printf_P(PSTR("yaw rate kI: %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
if(Serial.available() > 0){
@ -817,7 +827,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) {
return (0);
}
}
}
}
static int8_t
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();
while(1){
delay(250);
delay(100);
compass.read();
compass.calculate(0,0);
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
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,