fixed FBW bugs

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1560 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-26 07:14:54 +00:00
parent b8e53931b9
commit b4abbacbd8
7 changed files with 35 additions and 42 deletions

View File

@ -10,7 +10,7 @@
#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #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 full ACRO mode, set value to 0
// if you want safe ACRO mode, set value to 100 // if you want safe ACRO mode, set value to 100
// if you want mostly stabilize with flips, set value to 4200 // if you want mostly stabilize with flips, set value to 4200

View File

@ -863,6 +863,7 @@ void update_current_flight_mode(void)
case FBW: case FBW:
// we are currently using manual throttle during alpha testing. // we are currently using manual throttle during alpha testing.
fbw_timer++; fbw_timer++;
//call at 5 hz //call at 5 hz
if(fbw_timer > 20){ if(fbw_timer > 20){
fbw_timer = 0; fbw_timer = 0;
@ -877,8 +878,8 @@ void update_current_flight_mode(void)
dTnav = 200; dTnav = 200;
} }
next_WP.lat = home.lat + rc_1.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; // 10 meteres next_WP.lng = home.lng - rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres
} }
// Yaw control // Yaw control

View File

@ -273,7 +273,7 @@
#endif #endif
#ifndef ACRO_RATE_TRIGGER #ifndef ACRO_RATE_TRIGGER
# define ACRO_RATE_TRIGGER 4200 # define ACRO_RATE_TRIGGER 100
#endif #endif

View File

@ -153,7 +153,7 @@ set_servos_4()
num++; num++;
if (num > 10){ if (num > 10){
num = 0; num = 0;
Serial.print("!"); //Serial.print("!");
//debugging with Channel 6 //debugging with Channel 6
/* /*
@ -211,7 +211,7 @@ set_servos_4()
num++; num++;
if (num > 10){ if (num > 10){
num = 0; num = 0;
Serial.print("-"); //Serial.print("-");
} }
if(rc_3.control_in > 0){ if(rc_3.control_in > 0){

View File

@ -69,11 +69,11 @@ void calc_nav()
nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// rotate the vector // rotate the vector
nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y; nav_roll = (float)nav_lat * yawvector.x - (float)nav_lon * yawvector.y;
nav_pitch = (float)nav_lon * yawvector.y + (float)nav_lat * yawvector.x; 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_roll = constrain(nav_roll, -pitch_max, pitch_max);
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max); //nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
} }
/* /*

View File

@ -2,7 +2,7 @@ void init_rc_in()
{ {
read_EEPROM_radio(); // read Radio limits read_EEPROM_radio(); // read Radio limits
rc_1.set_angle(4500); 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.set_angle(4500);
rc_2.dead_zone = 60; rc_2.dead_zone = 60;
rc_3.set_range(0,1000); rc_3.set_range(0,1000);

View File

@ -164,24 +164,26 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); 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){ while(1){
delay(20); delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio(); read_radio();
if(rc_3.control_in > 0){ 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++; fail_test++;
} }
if(oldSwitchPosition != readSwitch()){ if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("MODE CHANGE: ")); Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]); Serial.println(flight_mode_strings[readSwitch()]);
fail_test++; fail_test++;
} }
if(throttle_failsafe_enabled && rc_3.get_failsafe()){ if(throttle_failsafe_enabled && rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in); Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in);
Serial.println(flight_mode_strings[readSwitch()]); 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 // custom code/exceptions for flight modes
// --------------------------------------- // ---------------------------------------
//update_current_flight_mode(); update_current_flight_mode();
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
//set_servos_4(); set_servos_4();
ts_num++; ts_num++;
if (ts_num > 10){ if (ts_num == 5){
dTnav = 200; // 10 hz
ts_num = 0;
//next_WP.lat = random(-3000, 3000); GPS.longitude = 0;
//next_WP.lng = random(-3000, 3000); GPS.latitude = 0;
next_WP.lat = 3000;
next_WP.lng = 3000;
GPS.longitude = 0;
GPS.latitude = 0;
calc_nav(); calc_nav();
ts_num = 0; Serial.printf_P(PSTR(" ys:%ld, next_WP.lat:%ld, next_WP.lng:%ld, n_lat:%ld, n_lon:%ld \n"),
Serial.printf_P(PSTR(" ys:%ld, ny:%ld, ye:%ld, n_lat %ld, n_lon %ld -- n_pit %ld, n_rll %ld\n"),
dcm.yaw_sensor, dcm.yaw_sensor,
nav_yaw, next_WP.lat,
yaw_error, next_WP.lng,
nav_lat, nav_lat,
nav_lon, nav_lon,
nav_pitch, nav_pitch,
nav_roll); 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){ if(Serial.available() > 0){
return (0); return (0);
} }
} }
} }
} }
static int8_t static int8_t
test_adc(uint8_t argc, const Menu::arg *argv) test_adc(uint8_t argc, const Menu::arg *argv)
{ {