From 4dcc1a905dfcfc9f4f4831627334d6aef7b22877 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 30 Jan 2011 02:36:03 +0000 Subject: [PATCH] FBW bug fixes git-svn-id: https://arducopter.googlecode.com/svn/trunk@1576 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 5 ++--- ArduCopterMega/ArduCopterMega.pde | 7 ++++--- ArduCopterMega/EEPROM.pde | 1 + ArduCopterMega/Log.pde | 29 +++++++++++++---------------- ArduCopterMega/config.h | 6 +++--- ArduCopterMega/control_modes.pde | 9 +-------- ArduCopterMega/setup.pde | 20 ++++++++++---------- ArduCopterMega/test.pde | 7 ++++--- 8 files changed, 38 insertions(+), 46 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index dba1ed4db0..44bc073ed2 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -5,9 +5,8 @@ #define GPS_PROTOCOL GPS_PROTOCOL_MTK #define GCS_PROTOCOL GCS_PROTOCOL_NONE -//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK -//#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD -#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD +#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK +//#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define ACRO_RATE_TRIGGER 4200 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index e021c15950..d7bd485f4a 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -571,9 +571,10 @@ void medium_loop() if (log_bitmask & MASK_LOG_NTUN) Log_Write_Nav_Tuning(); - if (log_bitmask & MASK_LOG_GPS) - Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats); - + if (log_bitmask & MASK_LOG_GPS){ + if(home_is_set) + Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats); + } send_message(MSG_ATTITUDE); // Sends attitude data break; diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index c840e240d2..9231019d4f 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -17,6 +17,7 @@ void read_EEPROM_startup(void) imu.load_accel_eeprom(); read_EEPROM_alt_RTL(); read_EEPROM_current(); + read_EEPROM_nav(); // magnatometer read_EEPROM_compass(); read_EEPROM_compass_declination(); diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 605040a1fa..d830831111 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -104,6 +104,7 @@ dump_log(uint8_t argc, const Menu::arg *argv) // check that the requested log number can be read dump_log = argv[1].i; last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); + if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) { Serial.printf_P(PSTR("bad log number\n")); return(-1); @@ -111,7 +112,8 @@ dump_log(uint8_t argc, const Menu::arg *argv) dump_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log-1)*0x02)); dump_log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(dump_log)*0x02))-1; - if (dump_log == last_log_num) { + + if (dump_log == last_log_num) { dump_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); } Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), @@ -345,21 +347,21 @@ void Log_Write_Current() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CURRENT_MSG); + DataFlash.WriteInt(rc_3.control_in); DataFlash.WriteInt((int)(current_voltage * 100.0)); DataFlash.WriteInt((int)(current_amps * 100.0)); - DataFlash.WriteInt((int)(current_total * 10.0)); + DataFlash.WriteInt((int)current_total); DataFlash.WriteByte(END_BYTE); } + // Read a Current packet void Log_Read_Current() { - Serial.print("CURR:"); - Serial.print((float)DataFlash.ReadInt() / 100.f); - Serial.print(comma); - Serial.print((float)DataFlash.ReadInt() / 100.f); - Serial.print(comma); - Serial.print((float)DataFlash.ReadInt() / 10.f); - Serial.println(); + Serial.printf("CURR: %d, %4.4f, %4.4f, %d\n", + DataFlash.ReadInt(), + ((float)DataFlash.ReadInt() / 100.f), + ((float)DataFlash.ReadInt() / 100.f), + DataFlash.ReadInt()); } // Read an control tuning packet @@ -474,17 +476,13 @@ void Log_Read_Attitude() // Read a mode packet void Log_Read_Mode() { - byte mode; - - mode = DataFlash.ReadByte(); Serial.print("MOD:"); - Serial.println(flight_mode_strings[control_mode]); + Serial.println(flight_mode_strings[DataFlash.ReadByte()]); } // Read a GPS packet void Log_Read_GPS() { - Serial.print("GPS:"); Serial.print(DataFlash.ReadLong()); // Time Serial.print(comma); @@ -503,12 +501,11 @@ void Log_Read_GPS() Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed Serial.print(comma); Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course - } // Read a raw accel/gyro packet void Log_Read_Raw() -{ +{ float logvar; Serial.print("RAW:"); for (int y=0;y<6;y++) { diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 3d6b277229..545dbe5525 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -355,7 +355,7 @@ // // how much to we pitch towards the target #ifndef PITCH_MAX -# define PITCH_MAX 22 +# define PITCH_MAX 21 #endif @@ -366,13 +366,13 @@ # define NAV_P 1.2 #endif #ifndef NAV_I -# define NAV_I 0.1 +# define NAV_I 0.01 #endif #ifndef NAV_D # define NAV_D 0.005 #endif #ifndef NAV_IMAX -# define NAV_IMAX 1200 +# define NAV_IMAX 250 #endif diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 23a3f7848f..63ff046094 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -4,10 +4,6 @@ void read_control_switch() //motor_armed = (switchPosition < 5); if (oldSwitchPosition != switchPosition){ - if(motor_armed) - Serial.println("motor_armed"); - else - Serial.println("motor disarmed"); set_mode(flight_modes[switchPosition]); @@ -76,16 +72,13 @@ void read_trim_switch() if(trim_flag){ // switch was just released if((millis() - trim_timer) > 2000){ - // not being used } else { - // set the throttle nominal if(rc_3.control_in > 50){ throttle_cruise = rc_3.control_in; - //Serial.print("tnom "); - //Serial.println(throttle_cruise, DEC); + Serial.printf("tnom %d\n", throttle_cruise); save_EEPROM_throttle_cruise(); } } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 857231e103..fcf6b93fbc 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -718,7 +718,7 @@ void report_current() print_enabled(current_enabled); Serial.printf_P(PSTR("mah: %d"),milliamp_hours); - print_blanks(2); + print_blanks(1); } @@ -740,7 +740,7 @@ void report_frame() Serial.printf_P(PSTR("HEXA ")); Serial.printf_P(PSTR("frame (%d)"), (int)frame_type); - print_blanks(2); + print_blanks(1); } void report_radio() @@ -751,7 +751,7 @@ void report_radio() // radio read_EEPROM_radio(); print_radio_values(); - print_blanks(2); + print_blanks(1); } void report_gains() @@ -789,7 +789,7 @@ void report_gains() print_PID(&pid_baro_throttle); Serial.printf_P(PSTR("sonar throttle:\n")); print_PID(&pid_sonar_throttle); - print_blanks(2); + print_blanks(1); } void report_xtrack() @@ -801,11 +801,11 @@ void report_xtrack() read_EEPROM_nav(); Serial.printf_P(PSTR("XTRACK: %4.2f\n" "XTRACK angle: %d\n" - "PITCH_MAX: %d"), + "PITCH_MAX: %ld"), x_track_gain, x_track_angle, pitch_max); - print_blanks(2); + print_blanks(1); } void report_throttle() @@ -825,7 +825,7 @@ void report_throttle() throttle_cruise, throttle_failsafe_enabled, throttle_failsafe_value); - print_blanks(2); + print_blanks(1); } void report_imu() @@ -836,7 +836,7 @@ void report_imu() imu.print_gyro_offsets(); imu.print_accel_offsets(); - print_blanks(2); + print_blanks(1); } void report_compass() @@ -860,7 +860,7 @@ void report_compass() mag_offset_x, mag_offset_y, mag_offset_z); - print_blanks(2); + print_blanks(1); } @@ -874,7 +874,7 @@ void report_flight_modes() for(int i = 0; i < 6; i++ ){ print_switch(i, flight_modes[i]); } - print_blanks(2); + print_blanks(1); } /***************************************************************************/ diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index d280db82da..6f59f1f617 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -355,16 +355,17 @@ test_fbw(uint8_t argc, const Menu::arg *argv) GPS.latitude = 0; calc_nav(); - 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, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "), dcm.yaw_sensor, next_WP.lat, next_WP.lng, nav_lat, nav_lon, nav_pitch, - nav_roll); + nav_roll, + pitch_max); - //print_motor_out(); + print_motor_out(); } if(Serial.available() > 0){