Show vibration in acc-fields

Uses a moving window to calculate the difference between acceleration
min and max values to determin the vibration level.

Author: chsw

MavLink_FrSkySPort/Average.ino

@@ -1,6 +1,14 @@
 uint16_t Volt_AverageBuffer[10]; 
 uint16_t Current_AverageBuffer[10]; 
 
+#define accBufferSize 5
+int32_t accXBuffer[accBufferSize];
+int32_t accYBuffer[accBufferSize];
+int32_t accZBuffer[accBufferSize];
+int nrSamplesX = 0;
+int nrSamplesY = 0;
+int nrSamplesZ = 0;
+
 //returns the average of Voltage for the 10 last values  
 uint32_t Get_Volt_Average(uint16_t value)  {
   uint8_t i;
@@ -27,3 +35,98 @@ uint32_t Get_Current_Average(uint16_t value)  {
   return (sum+=value)/10;
 }
 
+void storeAccX(int32_t value)
+{
+  if(nrSamplesX < accBufferSize)
+  {
+    nrSamplesX++;
+  }
+  uint8_t i;
+  for(i=accBufferSize-1;i>0;i--)
+  {
+    accXBuffer[i]=accXBuffer[i-1];
+  }
+  accXBuffer[0] = value;
+}
+void storeAccY(int32_t value)
+{
+  if(nrSamplesY < accBufferSize)
+  {
+    nrSamplesY++;
+  }
+  uint8_t i;
+  for(i=accBufferSize-1;i>0;i--)
+  {
+    accYBuffer[i]=accYBuffer[i-1];
+  }
+  accYBuffer[0] = value;
+}
+void storeAccZ(int32_t value)
+{
+  if(nrSamplesZ < accBufferSize)
+  {
+    nrSamplesZ++;
+  }
+  uint8_t i;
+  for(i=accBufferSize-1;i>0;i--)
+  {
+    accZBuffer[i]=accZBuffer[i-1];
+  }
+  accZBuffer[0] = value;
+}
+
+int32_t fetchAccX()
+{
+  int32_t min=32000;
+  int32_t max=-32000;
+  for(int i=0; i<nrSamplesX; i++)
+  {
+    if(accXBuffer[i]<min)
+    {
+      min = accXBuffer[i];
+    }
+    if(accXBuffer[i]>max)
+    {
+      max = accXBuffer[i];
+    }
+  }
+  return max - min;
+}
+
+int32_t fetchAccY()
+{
+  int32_t min=32000;
+  int32_t max=-32000;
+  for(int i=0; i<nrSamplesY; i++)
+  {
+    if(accYBuffer[i]<min)
+    {
+      min = accYBuffer[i];
+    }
+    if(accYBuffer[i]>max)
+    {
+      max = accYBuffer[i];
+    }
+  }
+  return max - min;
+}
+
+int32_t fetchAccZ()
+{
+  int32_t min=32000;
+  int32_t max=-32000;
+  for(int i=0; i<nrSamplesZ; i++)
+  {
+    if(accZBuffer[i]<min)
+    {
+      min = accZBuffer[i];
+    }
+    if(accZBuffer[i]>max)
+    {
+      max = accZBuffer[i];
+    }
+  }
+  return max - min;
+}
+
+

MavLink_FrSkySPort/FrSkySPort.ino

@@ -176,13 +176,13 @@ void FrSkySPort_ProcessSensorRequest(uint8_t sensorId)
       FrSkySPort_SendPackage(FR_ID_ADC2,adc2);                  
       break;       
     case 2:
-      FrSkySPort_SendPackage(FR_ID_ACCX,ap_accX_old - ap_accX);    
+      FrSkySPort_SendPackage(FR_ID_ACCX, fetchAccX());    
       break;
     case 3:
-      FrSkySPort_SendPackage(FR_ID_ACCY,ap_accY_old - ap_accY); 
+      FrSkySPort_SendPackage(FR_ID_ACCY, fetchAccY()); 
       break; 
     case 4:
-      FrSkySPort_SendPackage(FR_ID_ACCZ,ap_accZ_old - ap_accZ); 
+      FrSkySPort_SendPackage(FR_ID_ACCZ, fetchAccZ()); 
       break; 
     case 5:
       FrSkySPort_SendPackage(FR_ID_T1,gps_status); 

MavLink_FrSkySPort/MavLink_FrSkySPort.ino

@@ -90,15 +90,6 @@ uint16_t  ap_throttle = 0;
 int32_t    ap_bar_altitude = 0;    // 100 = 1m
 int32_t    ap_climb_rate=0;        // 100= 1m/s
 
-// Message #27 RAW IMU 
-int32_t   ap_accX = 0;
-int32_t   ap_accY = 0;
-int32_t   ap_accZ = 0;
-
-int32_t   ap_accX_old = 0;
-int32_t   ap_accY_old = 0;
-int32_t   ap_accZ_old = 0;
-
 // ******************************************
 // These are special for FrSky
 int32_t   adc2 = 0;               // 100 = 1.0V
@@ -115,7 +106,6 @@ uint16_t  hb_count;
 
 unsigned long MavLink_Connected_timer;
 unsigned long hb_timer;
-unsigned long acc_timer;
 
 int led = 13;
 
@@ -130,7 +120,6 @@ void setup()  {
   MavLink_Connected = 0;
   MavLink_Connected_timer=millis();
   hb_timer = millis();
-  acc_timer=millis();
   hb_count = 0;
 
 
@@ -176,15 +165,6 @@ void loop()  {
   FrSkySPort_Process();               // Check FrSky S.Port communication
 
   adc2 =analogRead(0)/4;               // Read ananog value from A0 (Pin 14). ( Will be A2 value on FrSky LCD)
-
-  if((millis() - acc_timer) > 1000) {    // Reset timer for AccX, AccY and AccZ
-    ap_accX_old=ap_accX;
-    ap_accY_old=ap_accY;
-    ap_accZ_old=ap_accZ;
-    acc_timer=millis();
-    //debugSerial.println(ap_base_mode);
-  }
-
 }
 
 
@@ -261,9 +241,10 @@ void _MavLink_receive() {
 #endif
         break;
       case MAVLINK_MSG_ID_RAW_IMU:   // 27
-        ap_accX = mavlink_msg_raw_imu_get_xacc(&msg) / 10;                // 
-        ap_accY = mavlink_msg_raw_imu_get_yacc(&msg) / 10;
-        ap_accZ = mavlink_msg_raw_imu_get_zacc(&msg) / 10;
+        storeAccX(mavlink_msg_raw_imu_get_xacc(&msg) / 10);
+        storeAccY(mavlink_msg_raw_imu_get_yacc(&msg) / 10);
+        storeAccZ(mavlink_msg_raw_imu_get_zacc(&msg) / 10);
+
 #ifdef DEBUG_ACC
         debugSerial.print(millis());
         debugSerial.print("\tMAVLINK_MSG_ID_RAW_IMU: xacc: ");
@@ -307,3 +288,4 @@ void _MavLink_receive() {
 
 
 
+