enhance the gyro calibration process

src/flight/sixaxis.c

@@ -136,8 +136,22 @@ void sixaxis_read() {
   }
 }
 
-static void sixaxis_wait_for_still() {
+static bool test_gyro_move(const gyro_data_t *last_data,  const gyro_data_t *data){
+  bool did_move = false;
+  for (uint8_t i = 0; i < 3; i++) {
+    const float delta = fabsf(fabsf(last_data->gyro.axis[i] * GYRO_RANGE) - fabsf(data->gyro.axis[i] * GYRO_RANGE));
+    if (delta > 0.3f) {
+      did_move = true;
+      break;
+    }
+  }
+  return did_move;
+}
+
+//returns true if it's already still, i.e. no move since the first loops
+static bool sixaxis_wait_for_still(uint32_t timeout) {
   const uint32_t start = time_micros();
+  int loop_counter = 0;
 
   // turn on led
   uint8_t move_counter = 15;
@@ -146,17 +160,10 @@ static void sixaxis_wait_for_still() {
   memset(&last_data, 0, sizeof(gyro_data_t));
 
   uint32_t now = start;
-  while (now - start < WAIT_TIME && move_counter > 0) {
+  while (now - start < timeout && move_counter > 0) {
     const gyro_data_t data = gyro_spi_read();
 
-    bool did_move = false;
-    for (uint8_t i = 0; i < 3; i++) {
-      const float delta = fabsf(fabsf(last_data.gyro.axis[i] * GYRO_RANGE) - fabsf(data.gyro.axis[i] * GYRO_RANGE));
-      if (delta > 0.3f) {
-        did_move = true;
-        break;
-      }
-    }
+    bool did_move = test_gyro_move(&last_data, &data);
 
     if (did_move) {
       move_counter = 15;
@@ -171,24 +178,29 @@ static void sixaxis_wait_for_still() {
 
     now = time_micros();
     last_data = data;
+    ++loop_counter;
   }
+  return loop_counter < 20;
 }
 
 void sixaxis_gyro_cal() {
-  float limit[3];
-  for (int i = 0; i < 3; i++) {
-    limit[i] = gyrocal[i];
+  for(int retry = 0; retry < 15 ; ++retry){
+    if(sixaxis_wait_for_still(WAIT_TIME / 15)){
+      //break only if it's already still, otherwise, wait and try again
+      break;
+    }
+    time_delay_ms(200);
   }
-
-  sixaxis_wait_for_still();
   gyro_spi_calibrate();
 
   uint8_t brightness = 0;
   led_pwm(brightness, 1000);
 
+  gyro_data_t last_data = gyro_spi_read();;
   uint32_t start = time_micros();
   uint32_t now = start;
-  while (now - start < CAL_TIME) {
+  int cal_counter = CAL_TIME / 1000;
+  for(int timeout = WAIT_TIME / 1000; timeout > 0; --timeout) {
     const gyro_data_t data = gyro_spi_read();
 
     led_pwm(brightness, 1000);
@@ -197,20 +209,21 @@ void sixaxis_gyro_cal() {
       brightness &= 0xF;
     }
 
-    for (uint8_t i = 0; i < 3; i++) {
-      if (data.gyro.axis[i] > limit[i])
-        limit[i] += 0.1f; // 100 gyro bias / second change
-      if (data.gyro.axis[i] < limit[i])
-        limit[i] -= 0.1f;
-
-      limit[i] = constrain(limit[i], -GYRO_BIAS_LIMIT, GYRO_BIAS_LIMIT);
-      lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6));
+    bool did_move = test_gyro_move(&last_data, &data);
+    if(!did_move){//only cali gyro when it's still
+      for (uint8_t i = 0; i < 3; i++) {
+        lpf(&gyrocal[i], data.gyro.axis[i], lpfcalc(1000, 0.5 * 1e6));
+      }
+      if(--cal_counter <= 0){
+        break;
+      }
     }
 
     while ((time_micros() - now) < 1000)
       ;
 
     now = time_micros();
+    last_data = data;
   }
 }
 

src/io/msp.c

@@ -116,6 +116,20 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,
     msp_send_reply(msp, magic, cmd, data, 12);
     break;
   }
+  case MSP_ANALOG: {
+    const int16_t current = (int16_t)constrain(state.ibat / 10, -320, 320);
+    const uint16_t rssi = (uint16_t)constrain(state.rx_rssi * 1023 / 100, 0, 1023);
+    const uint16_t vbat = (uint8_t)constrain(state.vbat_filtered / 0.01, 0, 255);
+    uint8_t data[9] = {
+        (uint8_t)constrain(state.vbat_filtered / 0.1, 0, 255), // battery voltage
+        0x0, 0x0,                                              // battery drawn in mAh
+        (rssi >> 8) & 0xFF, rssi & 0xFF,                       // rssi
+        (current >> 8) & 0xFF, current & 0xFF,                 // current in 0.01 A steps, range is -320A to 320A
+        (vbat >> 8) & 0xFF, vbat & 0xFF,                       // battery voltage
+    };
+    msp_send_reply(msp, magic, cmd, data, 9);
+    break;
+  }
   case MSP_BATTERY_STATE: {
     const uint16_t current = state.ibat / 1000;
     uint8_t data[9] = {

src/io/msp.h

@@ -23,6 +23,7 @@
 #define MSP_EEPROM_WRITE 250 // in message          no param
 #define MSP_REBOOT 68        // in message reboot settings
 
+#define MSP_ANALOG 110    //out message         vbat, powermetersum, rssi if available on RX
 #define MSP_BATTERY_STATE 130 // out message         Connected/Disconnected, Voltage, Current Used
 
 #define MSP_UID 160   // out message         Unique device ID