SDCARD logging

extras/BinLogTools/README.md

@@ -0,0 +1 @@
+This folder contains tools for ArduPilot Binary Logs

extras/BinLogTools/dump.py

@@ -0,0 +1,16 @@
+#dump bin log to stdout
+
+#pip install pymavlink
+from pymavlink import DFReader
+import sys
+
+filename = sys.argv[1]
+log = DFReader.DFReader_binary(filename)
+
+while True:
+    m = log.recv_msg()
+    if m is None:
+        break
+    #bout.write(m.get_msgbuf())
+    print(m)
+

extras/BinLogTools/stats.py

@@ -0,0 +1,107 @@
+#print bin log statistics - does not depend on pymavlink
+
+import sys
+
+if len(sys.argv) < 2: 
+  print("ERROR: need filename on command line")
+  exit()
+
+filename = sys.argv[1]
+
+fh = open(filename, 'rb')
+#d = bytearray(fh.read())
+d = fh.read()
+fh.close()
+
+dlen = len(d)
+
+name = [''] * 256
+fmt = [''] * 256
+cols = [''] * 256
+len = [0] * 256
+cnt = [0] * 256
+fmtpos = [0] * 256
+recpos = [0] * 256
+
+miss_cnt = 0
+
+i = 0
+
+while i<dlen:
+#    if(d[i]==0xa3 and d[i+1]==0x95): print("\n")
+#    print("{:02X} ".format(d[i]), end="")
+
+    if d[i]==0xa3 and d[i+1]==0x95 and d[i+2]==0x80 :
+        t = d[i+3]
+        len[t] = d[i+4]
+        name[t] = d[i+5:i+5+4].decode("ascii").rstrip('\x00')
+        fmt[t] = d[i+9:i+9+16].decode("ascii").rstrip('\x00')
+        cols[t] = d[i+25:i+25+64].decode("ascii").rstrip('\x00')
+        cnt[0x80] += 1
+        fmtpos[t] = i
+        #print("{:5d} FMT: typ:{:02X} len:{:2d} name:{:<4}".format(i,t,ln,n))
+        i += len[0x80]
+    elif d[i]==0xa3 and d[i+1]==0x95 :
+        t = d[i+2]
+        n = name[t]
+        ln = len[t]
+        cnt[t] += 1
+        if recpos[t]==0: 
+            recpos[t]=i
+            #print("{:5d} {}: len:{:2d}".format(i, n, ln))
+        i += ln
+    else:
+        miss_cnt += 1
+        #print("{:02X} ".format(d[i]), end="")
+        i += 1
+
+#print unused types
+typ_cnt = 0
+for i in range(256):
+    if name[i] != '' and cnt[i]==0: typ_cnt += 1
+if typ_cnt>0:
+    print("==== UNUSED ===")
+    print("Msg   Count Type Len Format           Columns")
+    print("---- ------ ---- --- ---------------- -----------------------------------------")
+    max_len = 0
+    for i in range(256):
+        if name[i] != '' and cnt[i]==0:
+            if max_len<len[i]: max_len = len[i]    
+            #print("{:02X} len:{:3d} cnt:{:6d} fmtpos:{:6d} recpos:{:6d} name:{}".format(i,len[i],cnt[i],fmtpos[i],recpos[i],name[i]))
+        print("{: <4} {:6d} 0x{:02X} {:3d} {: <16} {}".format(name[i],cnt[i],i,len[i],fmt[i],cols[i]))
+    print("Type Count     :",typ_cnt)
+    print("Max Record Len :",max_len)
+    print("");
+    print("==== UNUSED ===")
+
+#print used types
+print("Msg   Count Type Len Format           Columns")
+print("---- ------ ---- --- ---------------- -----------------------------------------")
+typ_cnt = 0
+rec_cnt = 0
+max_len = 0
+for i in range(256):
+    if name[i] != '' and cnt[i]>0:
+        typ_cnt += 1
+        rec_cnt += cnt[i]
+        if max_len<len[i]: max_len = len[i]
+        #print("{:02X} len:{:3d} cnt:{:6d} fmtpos:{:6d} recpos:{:6d} name:{}".format(i,len[i],cnt[i],fmtpos[i],recpos[i],name[i]))
+        print("{: <4} {:6d} 0x{:02X} {:3d} {: <16} {}".format(name[i],cnt[i],i,len[i],fmt[i],cols[i]))
+print("Type Count     :",typ_cnt)
+print("Max Record Len :",max_len)
+print("Record Count   :",rec_cnt)
+print("Parse Errors   :",miss_cnt)
+
+
+"""
+  struct PACKED {
+      uint8_t h1 = HEAD_BYTE1;
+      uint8_t h2 = HEAD_BYTE2;
+      uint8_t type = 0x80;
+      uint8_t msg_type;
+      uint8_t length;
+      char name[4];
+      char format[16];
+      char labels[64];
+  } FMT;
+"""

library.properties

@@ -1,9 +1,9 @@
 name=madflight
-version=1.2.0
+version=1.2.1
 author=qqqlab
 maintainer=qqqlab <[email protected]>
 license=GNU
-sentence=Flight Controller for ESP32 / RP2350 / RP2040 / STM32
+sentence=Flight Controller for ESP32-S3 / ESP32 / RP2350 / RP2040 / STM32
 paragraph=Flight tested with readily available development boards and sensor breakout boards. Build a quadcopter, helicoper, airplane or VTOL craft. Includes modules for PID control, CRSF/ELRS/DSMX/SBUS receiver and telemetry, AHRS, GPS, barometer, magnetometer, calibration, command line interface, black box data logging.
 category=Other
 url=https://madflight.com/arduino

src/madflight.h

@@ -1,4 +1,4 @@
-#define MADFLIGHT_VERSION "madflight v1.2.1-DEV"
+#define MADFLIGHT_VERSION "madflight v1.2.1"
 
 /*==========================================================================================
 madflight - Flight Controller for ESP32 / RP2040 / STM32

src/madflight/baro/baro.h

@@ -140,20 +140,38 @@ BarometerMS5611 baro_sensor;
 
 #include "../interface.h"
 
-int Barometer::setup(uint32_t sampleRate) {
+int Barometer::setup(uint32_t sampleRate, float filterAltHertz, float filterVzHertz) {
   _sampleRate = sampleRate;
   _samplePeriod = 1000000 / sampleRate;
+  B_alt = constrain(1 - exp(-2 * PI * filterAltHertz / sampleRate), 0.0f, 1.0f);
+  B_vz = constrain(1 - exp(-2 * PI * filterVzHertz / sampleRate), 0.0f, 1.0f);
   dt = 0;
   ts = micros();
-  return baro_sensor.setup();
+  int rv = baro_sensor.setup();
+  if(rv != 0) {
+    press = 0;
+    temp = 0;
+    alt = 0;
+    altRaw = 0;
+    vz = 0;
+  }else{
+    update(); //get first reading
+    alt = altRaw; //jumpstart filtered altitude
+  }
+  Serial.printf("BARO: sample_rate=%dHz filt_alt=%.1fHz filt_vz=%.1fHz rv=%d \n", (int)sampleRate, filterAltHertz, filterVzHertz, rv);
+  return rv;
 }
 
 bool Barometer::update() {
   if (micros() - ts >= _samplePeriod) {
-    baro_sensor.update(&press, &temp);
-    alt = (101325.0 - press) / 12.0;
     uint32_t tsnew = micros();
     dt = (tsnew - ts) / 1000000.0;
+    baro_sensor.update(&press, &temp);
+    altRaw = (101325.0 - press) / 12.0;
+    float altnew = (1.0 - B_alt) * alt + B_alt * altRaw; //Low-pass filtered altitude
+    float vznew = (altnew - alt) / dt;
+    vz = (1.0 - B_vz) * vz + B_vz * vznew; //Low-pass filtered velocity
+    alt = altnew;
     ts = tsnew;
     return true;
   }

src/madflight/bb/bb_sdcard/BBFS_SD_RP2040.h

@@ -82,7 +82,9 @@ class BBFS_SD : public BBFS {
   }
 
   void write(const uint8_t *buf, const uint8_t len) override {
-    for(int i=0;i<len;i++) writeChar(buf[i]);
+    for(int i=0;i<len;i++) {
+      writeChar(buf[i]);
+    }
   }
 
   void close() override {
@@ -124,7 +126,11 @@ class BBFS_SD : public BBFS {
           Serial.println("BB: bench - malloc failed");
           return;
       }
-
+
+      if(SD.exists(path)) {
+        SD.remove(path);
+      }
+
       file = SD.open(path, FILE_WRITE);
       if(!file){
           Serial.println("BB: bench - Failed to open file for writing");