fixed a bug on Yaw.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1309 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-28 01:45:43 +00:00
parent 94ebc7860c
commit 10036c7277
2 changed files with 8 additions and 10 deletions

View File

@ -8,7 +8,6 @@
void read_EEPROM_startup(void)
{
read_EEPROM_PID();
read_EEPROM_radio(); // read Radio limits
read_EEPROM_frame();
read_EEPROM_configs();
read_EEPROM_flight_modes();
@ -16,6 +15,7 @@ void read_EEPROM_startup(void)
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
read_EEPROM_alt_RTL();
// magnatometer
read_EEPROM_mag();
read_EEPROM_mag_declination();

View File

@ -1,12 +1,12 @@
void init_rc_in()
{
read_EEPROM_radio(); // read Radio limits
rc_1.set_angle(4500);
rc_1.dead_zone = 50;
rc_2.set_angle(4500);
rc_2.dead_zone = 50;
rc_3.set_range(0,1000);
rc_3.dead_zone = 20;
//rc_3.radio_max += 300; // hack for better throttle control
rc_3.scale_output = .8;
rc_4.set_angle(6000);
rc_4.dead_zone = 500;
@ -15,7 +15,6 @@ void init_rc_in()
rc_6.set_range(200,800);
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
}
void init_rc_out()
@ -52,7 +51,7 @@ void read_radio()
void trim_radio()
{
for (byte i = 0; i < 50; i++){
for (byte i = 0; i < 30; i++){
read_radio();
}
rc_1.trim(); // roll
@ -62,7 +61,7 @@ void trim_radio()
void trim_yaw()
{
for (byte i = 0; i < 50; i++){
for (byte i = 0; i < 30; i++){
read_radio();
}
rc_4.trim(); // yaw
@ -162,20 +161,19 @@ void set_servos_4(void)
//*/
//
/* debugging and dynamic kP
num++;
if (num > 50){
num = 0;
//Serial.printf("control_in: %d ", rc_3.control_in);
//Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out);
//Serial.printf(" cwm: %d %d %d %d, %d\t", rc_1.pwm_out, rc_2.pwm_out, rc_3.pwm_out, rc_4.pwm_out, rc_3.radio_out);
//Serial.printf(" out: %d %d %d %d\n", r_out, l_out, f_out, b_out);
//Serial.printf(" pwm: %d, %d %d %d %d\n",rc_3.pwm_out, motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
pid_stabilize_roll.kP((float)rc_6.control_in / 1000);
stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25;
init_pids();
//Serial.print("nav_yaw: ");
//Serial.println(nav_yaw,DEC);
//Serial.print("kP: ");
//Serial.println(pid_stabilize_roll.kP(),3);
}