diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d4a2c69f0a..94fa3f494a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -347,7 +347,6 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -//static long bearing_error; // deg * 100 : 0 to 36000 static long altitude_error; // meters * 100 we are off in altitude static long old_altitude; static long yaw_error; // how off are we pointed @@ -374,7 +373,7 @@ static int ground_temperature; // Altitude Sensor variables // ---------------------- static int sonar_alt; -static int baro_alt;; +static int baro_alt; static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR static int altitude_rate; @@ -476,7 +475,6 @@ static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm // ---------------------- static long perf_mon_timer; //static float imu_health; // Metric based on accel gain deweighting -static int G_Dt_max; // Max main loop cycle time in milliseconds static int gps_fix_count; static byte gps_watchdog; @@ -533,6 +531,7 @@ void loop() // reads all of the necessary trig functions for cameras, throttle, etc. update_trig(); + // perform 10hz tasks medium_loop(); // Stuff to run at full 50hz, but after the loops @@ -551,7 +550,8 @@ void loop() if (g.log_bitmask & MASK_LOG_PM) Log_Write_Performance(); - resetPerfData(); + gps_fix_count = 0; + perf_mon_timer = millis(); } //PORTK &= B10111111; } @@ -576,11 +576,6 @@ static void fast_loop() // IMU DCM Algorithm read_AHRS(); - // Look for slow loop times - // ------------------------ - //if (delta_ms_fast_loop > G_Dt_max) - // G_Dt_max = delta_ms_fast_loop; - // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1156,7 +1151,6 @@ static void update_navigation() }else{ // lets just jump to Loiter Mode after RTL set_mode(LOITER); - //xtrack_enabled = false; } // calculates the desired Roll and Pitch diff --git a/ArduCopter/GCS_Standard.pde b/ArduCopter/GCS_Standard.pde index 6c9de81131..8de2260c51 100644 --- a/ArduCopter/GCS_Standard.pde +++ b/ArduCopter/GCS_Standard.pde @@ -147,7 +147,7 @@ static void send_message(byte id, long param) { tempint = mainLoop_count; // Main Loop cycles mess_buffer[7] = tempint & 0xff; mess_buffer[8] = (tempint >> 8) & 0xff; - mess_buffer[9] = G_Dt_max & 0xff; + mess_buffer[9] = 0 & 0xff; mess_buffer[10] = gyro_sat_count; // Problem counts mess_buffer[11] = adc_constraints; mess_buffer[12] = renorm_sqrt_count; diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 98dbf41998..ed60a69aa9 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -658,8 +658,6 @@ static void Log_Write_Performance() //* //DataFlash.WriteLong( millis()- perf_mon_timer); - //DataFlash.WriteInt ( mainLoop_count); - DataFlash.WriteInt ( G_Dt_max); //1 DataFlash.WriteByte( dcm.gyro_sat_count); //2 DataFlash.WriteByte( imu.adc_constraints); //3 @@ -676,14 +674,13 @@ static void Log_Write_Performance() // Read a performance packet static void Log_Read_Performance() { - Serial.printf_P(PSTR( "PM, %d, %d, %d, " + Serial.printf_P(PSTR( "PM, %d, %d, " "%d, %d, %d, " "%d, %ld\n"), // Control //DataFlash.ReadLong(), //DataFlash.ReadInt(), - DataFlash.ReadInt(), //1 DataFlash.ReadByte(), //2 DataFlash.ReadByte(), //3 diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index 0514a74f64..62cc5df577 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -28,11 +28,10 @@ void roll_flip() AAP_timer = 0; AAP_state++; break; - case 1: // Step 2 : Increase throttle to start maneuver if (AAP_timer < 95){ // .5 seconds g.rc_1.servo_out = get_stabilize_roll(0); - g.rc_3.servo_out = get_throttle(g.rc_3.control_in + AAP_THR_INC); + g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; //g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); AAP_timer++; }else{ @@ -45,7 +44,7 @@ void roll_flip() if (dcm.roll_sensor < 4500){ // Roll control g.rc_1.servo_out = AAP_ROLL_OUT; //get_rate_roll(AAP_ROLL_RATE); - g.rc_3.servo_out = get_throttle(g.rc_3.control_in - AAP_THR_DEC); + g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; }else{ AAP_state++; } @@ -54,7 +53,7 @@ void roll_flip() case 3: // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45ยบ]) if ((dcm.roll_sensor >= 4500) || (dcm.roll_sensor < -4500)){ g.rc_1.servo_out = 150; //get_rate_roll(AAP_ROLL_RATE); - g.rc_3.servo_out = get_throttle(g.rc_3.control_in - AAP_THR_DEC); + g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; }else{ AAP_state++; } @@ -63,7 +62,7 @@ void roll_flip() case 4: // Step 5 : Increase throttle to stop the descend if (AAP_timer < 90){ // .5 seconds g.rc_1.servo_out = get_stabilize_roll(0); - g.rc_3.servo_out = get_throttle(g.rc_3.control_in + AAP_THR_INC); + g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; AAP_timer++; }else{ AAP_state++; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b7777c920f..8812079962 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -470,14 +470,6 @@ static void set_failsafe(boolean mode) } - -static void resetPerfData(void) { - //mainLoop_count = 0; - G_Dt_max = 0; - gps_fix_count = 0; - perf_mon_timer = millis(); -} - static void init_compass() {