Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/APM_Config.h
View File

@ -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

5
ArduCopterMega/ArduCopterMega.pde
View File

@ -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

2
ArduCopterMega/config.h
View File

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

4
ArduCopterMega/motors.pde
View File

@ -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){

10
ArduCopterMega/navigation.pde
View File

@ -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);
}
/*

2
ArduCopterMega/radio.pde
View File

@ -2,7 +2,7 @@ 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_3.set_range(0,1000);

44
ArduCopterMega/test.pde
View File

@ -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;
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)
{