From 9b96a18ad406b29176cc8529a7ac2622b1d11c30 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Thu, 14 Apr 2011 05:56:39 +0000 Subject: [PATCH] Removed all calls to legacy trim_radio(). Handled by radio setup. Added AP_Var Fingerprint checker. Will not let users fly with OOD firmware. Tweaked Yaw to give better response. Let me know how it goes. It looks fine, but I've not flown it. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1878 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 3 ++- ArduCopterMega/Attitude.pde | 37 +++++++++++-------------------- ArduCopterMega/motors.pde | 8 +++++-- ArduCopterMega/navigation.pde | 6 +---- ArduCopterMega/setup.pde | 5 ----- ArduCopterMega/system.pde | 18 +++++++++++++-- ArduCopterMega/test.pde | 10 --------- 7 files changed, 38 insertions(+), 49 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index ce6a222165..fefbf13730 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -43,7 +43,7 @@ version 2.1 of the License, or (at your option) any later version. #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions -//#include + // Configuration @@ -178,6 +178,7 @@ GPS *g_gps; GCS_Class gcs; #endif +//#include //GCS_SIMPLE gcs_simple(&Serial); AP_RangeFinder_MaxsonarXL sonar; diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 40f46142bf..674f906eef 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -139,13 +139,6 @@ output_yaw_with_hold(boolean hold) // Apply PID and save the new angle back to RC_Channel g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000 - // We adjust the output by the rate of rotation - long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 - int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 - - // Limit dampening to be equal to propotional term for symmetry - g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 - }else{ // rate control @@ -155,33 +148,29 @@ output_yaw_with_hold(boolean hold) // -error = CCW, +error = CW if(g.rc_4.control_in == 0){ - // we are breaking; - g.rc_4.servo_out = (omega.z > 0) ? -1800 : 1800; - - //g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0)); - - //switch comments to get old behavior. - //g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520 + //g.rc_4.servo_out = (omega.z > 0) ? -600 : 600; + g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0)); }else{ + g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520 - - // We adjust the output by the rate of rotation - //long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 - //int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 - - // Limit dampening to be equal to propotional term for symmetry - //g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 - g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° } - } + // We adjust the output by the rate of rotation + long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 + int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 + + // Limit dampening to be equal to propotional term for symmetry + g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 + + // Limit Output + g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° + //Serial.printf("%d\n",g.rc_4.servo_out); } -// slight left rudder gives right roll. void output_rate_roll() { diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 90e82a9e88..8b5bff50f8 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -29,7 +29,6 @@ void arm_motors() }else{ arming_counter = 0; } - }else{ arming_counter = 0; } @@ -55,7 +54,7 @@ set_servos_4() 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 + 50; + out_min = g.rc_3.radio_min + 60; //Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out); @@ -65,6 +64,10 @@ set_servos_4() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); + // limit Yaw control so we don't clip and loose altitude + // this is only a partial solution. + g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min)); + //Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out); //Serial.printf("yaw: %d ", g.rc_4.radio_out); @@ -241,6 +244,7 @@ set_servos_4() gcs_simple.write_int((int)nav_roll); gcs_simple.write_int((int)nav_pitch); + gcs_simple.write_long(current_loc.lat); //28 gcs_simple.write_long(current_loc.lng); //32 gcs_simple.write_int((int)current_loc.alt); //34 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 819a7b452b..746e489edb 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -54,6 +54,7 @@ void calc_loiter_nav() Becuase we are using lat and lon to do our distance errors here's a quick chart: 100 = 1m 1000 = 11m + 1800 = 1980m = 60 feet 3000 = 33m 10000 = 111m pitch_max = 22° (2200) @@ -89,26 +90,21 @@ void calc_waypoint_nav() //nav_lat = max(wp_distance, -DIST_ERROR_MAX); //nav_lat = min(wp_distance, DIST_ERROR_MAX); - //Serial.printf("nav_lat %ld, ", nav_lat); // Scale response by kP nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° - //Serial.printf("%ld, ",nav_lat); // get the sin and cos of the bearing error - rotated 90° sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); - //Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y); // rotate the vector nav_roll = (float)nav_lat * cos_nav_x; nav_pitch = -(float)nav_lat * sin_nav_y; - //Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch); long pmax = g.pitch_max.get(); nav_roll = constrain(nav_roll, -pmax, pmax); nav_pitch = constrain(nav_pitch, -pmax, pmax); - //Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch); } void calc_bearing_error() diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index fdc5bdee0c..f982149b30 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -202,10 +202,6 @@ setup_motors(uint8_t argc, const Menu::arg *argv) init_rc_in(); - // read the radio to set trims - // --------------------------- - trim_radio(); - print_hit_enter(); delay(1000); @@ -382,7 +378,6 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); - trim_radio(); while(1){ delay(20); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 3ac5447dae..1772be3ab8 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -107,7 +107,21 @@ void init_ardupilot() // save the new format version g.format_version.set_and_save(Parameters::k_format_version); - //Serial.println_P(PSTR("done.")); + Serial.printf_P(PSTR("Please Run Setup...\n")); + while (true) { + delay(100); + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, HIGH); + digitalWrite(C_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + } + motor_light = !motor_light; + } + }else{ unsigned long before = micros(); // Load all auto-loaded EEPROM variables @@ -245,7 +259,7 @@ void startup_ground(void) // read the radio to set trims // --------------------------- - trim_radio(); + //trim_radio(); if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Startup(TYPE_GROUNDSTART_MSG); diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index d8a0aebf11..6e28b2ddc5 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -129,10 +129,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - // read the radio to set trims - // --------------------------- - trim_radio(); - while(1){ delay(20); read_radio(); @@ -183,10 +179,6 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) read_radio(); } - // read the radio to set trims - // --------------------------- - trim_radio(); - oldSwitchPosition = readSwitch(); Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); @@ -243,8 +235,6 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP()); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); - trim_radio(); - motor_auto_safe = false; motor_armed = true;