From b4abbacbd8b38356305e68a77357ffbbe3b4e1f1 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Wed, 26 Jan 2011 07:14:54 +0000 Subject: [PATCH] fixed FBW bugs git-svn-id: https://arducopter.googlecode.com/svn/trunk@1560 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 2 +- ArduCopterMega/ArduCopterMega.pde | 5 ++-- ArduCopterMega/config.h | 2 +- ArduCopterMega/motors.pde | 4 +-- ArduCopterMega/navigation.pde | 10 +++---- ArduCopterMega/radio.pde | 4 +-- ArduCopterMega/test.pde | 50 +++++++++++++------------------ 7 files changed, 35 insertions(+), 42 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index a8a0c96732..dba1ed4db0 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -10,7 +10,7 @@ #define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD -#define ACRO_RATE_TRIGGER 4200 +//#define ACRO_RATE_TRIGGER 4200 // if you want full ACRO mode, set value to 0 // if you want safe ACRO mode, set value to 100 // if you want mostly stabilize with flips, set value to 4200 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 3667ac1345..2ca06ccc12 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -863,6 +863,7 @@ void update_current_flight_mode(void) case FBW: // we are currently using manual throttle during alpha testing. fbw_timer++; + //call at 5 hz if(fbw_timer > 20){ fbw_timer = 0; @@ -877,8 +878,8 @@ void update_current_flight_mode(void) dTnav = 200; } - next_WP.lat = home.lat + rc_1.control_in / 5; // 10 meteres - next_WP.lng = home.lng - rc_2.control_in / 5; // 10 meteres + next_WP.lat = home.lat + rc_1.control_in / 5; // 4500 / 5 = 900 = 10 meteres + next_WP.lng = home.lng - rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres } // Yaw control diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 72e445deb9..3d6b277229 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -273,7 +273,7 @@ #endif #ifndef ACRO_RATE_TRIGGER -# define ACRO_RATE_TRIGGER 4200 +# define ACRO_RATE_TRIGGER 100 #endif diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 8b56d4793d..8204edbb10 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -153,7 +153,7 @@ set_servos_4() num++; if (num > 10){ num = 0; - Serial.print("!"); + //Serial.print("!"); //debugging with Channel 6 /* @@ -211,7 +211,7 @@ set_servos_4() num++; if (num > 10){ num = 0; - Serial.print("-"); + //Serial.print("-"); } if(rc_3.control_in > 0){ diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 0502ba928b..117be182c5 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -69,11 +69,11 @@ void calc_nav() nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command // rotate the vector - nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y; - nav_pitch = (float)nav_lon * yawvector.y + (float)nav_lat * yawvector.x; - - nav_roll = constrain(nav_roll, -pitch_max, pitch_max); - nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max); + nav_roll = (float)nav_lat * yawvector.x - (float)nav_lon * yawvector.y; + nav_pitch = (float)nav_lon * yawvector.x + (float)nav_lat * yawvector.y; + Serial.printf("vx %4.4f,vy %4.4f, nr %ld, np %ld ", yawvector.x, yawvector.y, nav_roll, nav_pitch); + //nav_roll = constrain(nav_roll, -pitch_max, pitch_max); + //nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max); } /* diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 87e6c5830b..874aa7c3ee 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -2,9 +2,9 @@ void init_rc_in() { read_EEPROM_radio(); // read Radio limits rc_1.set_angle(4500); - rc_1.dead_zone = 60; + rc_1.dead_zone = 60; // 60 = .6 degrees rc_2.set_angle(4500); - rc_2.dead_zone = 60; + rc_2.dead_zone = 60; rc_3.set_range(0,1000); rc_3.dead_zone = 20; rc_3.scale_output = .9; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 2f56f20d92..26dc46349b 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -164,24 +164,26 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) oldSwitchPosition = readSwitch(); Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); + while(rc_3.control_in > 0){ + delay(20); + read_radio(); + } while(1){ delay(20); - - // Filters radio input - adjust filters in the radio.pde file - // ---------------------------------------------------------- read_radio(); if(rc_3.control_in > 0){ - Serial.printf_P(PSTR("THROTTLE ERROR %d \n"), rc_3.control_in); + Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), rc_3.control_in); fail_test++; } if(oldSwitchPosition != readSwitch()){ - Serial.printf_P(PSTR("MODE CHANGE: ")); + Serial.printf_P(PSTR("CONTROL MODE CHANGED: ")); Serial.println(flight_mode_strings[readSwitch()]); fail_test++; } + if(throttle_failsafe_enabled && rc_3.get_failsafe()){ Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in); Serial.println(flight_mode_strings[readSwitch()]); @@ -339,49 +341,39 @@ test_fbw(uint8_t argc, const Menu::arg *argv) // custom code/exceptions for flight modes // --------------------------------------- - //update_current_flight_mode(); + update_current_flight_mode(); // write out the servo PWM values // ------------------------------ - //set_servos_4(); + set_servos_4(); ts_num++; - if (ts_num > 10){ - dTnav = 200; - - //next_WP.lat = random(-3000, 3000); - //next_WP.lng = random(-3000, 3000); - next_WP.lat = 3000; - next_WP.lng = 3000; - - GPS.longitude = 0; - GPS.latitude = 0; + if (ts_num == 5){ + // 10 hz + ts_num = 0; + GPS.longitude = 0; + GPS.latitude = 0; calc_nav(); - - ts_num = 0; - Serial.printf_P(PSTR(" ys:%ld, ny:%ld, ye:%ld, n_lat %ld, n_lon %ld -- n_pit %ld, n_rll %ld\n"), + + Serial.printf_P(PSTR(" ys:%ld, next_WP.lat:%ld, next_WP.lng:%ld, n_lat:%ld, n_lon:%ld \n"), dcm.yaw_sensor, - nav_yaw, - yaw_error, + next_WP.lat, + next_WP.lng, nav_lat, nav_lon, nav_pitch, nav_roll); + + //print_motor_out(); } - //r: 0, p:0 -- ny:8000, ys:2172, ye:0, n_lat 0, n_lon 0 -- n_pit 0, n_rll 0 - - // R: 1417, L: 1453 F: 1453 B: 1417 - - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); - //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100)); if(Serial.available() > 0){ return (0); } - } } } + static int8_t test_adc(uint8_t argc, const Menu::arg *argv) {