diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 6e3f297e83..e774984e64 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -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 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 3d5424492c..5cde6e3589 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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(¤t_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); } - - - - diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 1c4f5b542e..0242933480 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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 diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 098ca2f843..9d7e7585a6 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -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); diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 5cf6a69118..6245495c91 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -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 flight_modes; + // Flight modes + // + AP_VarA 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 diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index cc025f9453..77ef8be653 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -207,8 +207,6 @@ void set_next_WP(struct Location *wp) // ------------------------------- void init_home() { - SendDebugln("MSG: init home"); - // block until we get a good fix // ----------------------------- while (!g_gps->new_data || !g_gps->fix) { diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 33aef85dbd..63ef9b203b 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 + + ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 6f6758dd09..76c607349b 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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() diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 68c62124e9..519e44bdbb 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/leds.pde b/ArduCopterMega/leds.pde index 2d77baf2e5..258ada4761 100644 --- a/ArduCopterMega/leds.pde +++ b/ArduCopterMega/leds.pde @@ -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 + diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index cf2768620f..a41fa638d7 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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++; diff --git a/ArduCopterMega/motors_octa_quad.pde b/ArduCopterMega/motors_octa_quad.pde new file mode 100644 index 0000000000..03c82a9db8 --- /dev/null +++ b/ArduCopterMega/motors_octa_quad.pde @@ -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 + diff --git a/ArduCopterMega/motors_y6.pde b/ArduCopterMega/motors_y6.pde index 2cd5aa426c..194862e2a6 100644 --- a/ArduCopterMega/motors_y6.pde +++ b/ArduCopterMega/motors_y6.pde @@ -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 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index eb5e082084..ec1b64d7f6 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -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); } } diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index f84fce2bfe..25ee3265c0 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -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() diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 2bc3e2e71d..d5fa66cf91 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -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() diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index b2f31f4554..93fe2c55f0 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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: diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 075b4eccfc..e943fdea7e 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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,