From 7229c169365724d8e0073441956f3ab9419b30cb Mon Sep 17 00:00:00 2001
From: Sean Kovacs <sean@techventures.biz>
Date: Mon, 7 Jul 2025 22:58:20 -0400
Subject: [PATCH 01/19] AP_GPS: Add support for quectel gps modules

Remove setting of data found in standard messages

Added debug logging for quectel module if logging enabled

added quectel bits to the python scripts

fix merge conflict

Update libraries/AP_GPS/AP_GPS_NMEA.h

Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>

fix other merge conflict

removing this to fallback to default
---
 Tools/scripts/build_options.py    |   1 +
 Tools/scripts/extract_features.py |   4 +
 libraries/AP_GPS/AP_GPS.cpp       |   3 +
 libraries/AP_GPS/AP_GPS.h         |   1 +
 libraries/AP_GPS/AP_GPS_NMEA.cpp  | 280 ++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS_NMEA.h    |  59 +++++++
 libraries/AP_GPS/AP_GPS_config.h  |   4 +
 7 files changed, 352 insertions(+)

diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index e27297f52d2791..af1095f3240c19 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -413,6 +413,7 @@ def config_option(self):
     Feature('GPS Drivers', 'GSOF', 'AP_GPS_GSOF_ENABLED', 'Enable GSOF GPS', 0, None),
     Feature('GPS Drivers', 'NMEA_GPS', 'AP_GPS_NMEA_ENABLED', 'Enable NMEA GPS', 0, None),
     Feature('GPS Drivers', 'NMEA_UNICORE', 'AP_GPS_NMEA_UNICORE_ENABLED', 'Enable NMEA Unicore GPS', 0, "NMEA_GPS"),
+    Feature('GPS Drivers', 'NMEA_QUECTEL', 'AP_GPS_NMEA_QUECTEL_ENABLED', 'Enable NMEA Quectel GPS', 0, "NMEA_GPS"),
     Feature('GPS Drivers', 'MAV', 'AP_GPS_MAV_ENABLED', 'Enable MAVLink GPS', 0, None),
     Feature('GPS Drivers', 'NOVA', 'AP_GPS_NOVA_ENABLED', 'Enable NOVA GPS', 0, None),
     Feature('GPS Drivers', 'SBF', 'AP_GPS_SBF_ENABLED', 'Enable SBF GPS', 0, None),
diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index 61fad4d1d35474..f720619c7ebd42 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -91,6 +91,10 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_RANGEFINDER_HEXSOONRADAR_ENABLED', r'AP_RangeFinder_NRA24_CAN::handle_frame'),
 
             ('AP_GPS_NMEA_UNICORE_ENABLED', r'AP_GPS_NMEA::parse_agrica_field',),
+            ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmver_field',),
+            ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmepe_field',),
+            ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmvel_field',),
+            ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmpvt_field',),
             ('AP_GPS_{type}_ENABLED', r'AP_GPS_(?P<type>.*)::read\b',),
 
             ('AP_OPTICALFLOW_ENABLED', 'AP_OpticalFlow::AP_OpticalFlow',),
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 3c083165a207b6..8aa2eecafd103b 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -808,6 +808,9 @@ AP_GPS_Backend *AP_GPS::_detect_instance(const uint8_t instance)
 #if AP_GPS_NMEA_UNICORE_ENABLED
                     type == GPS_TYPE_UNICORE_NMEA ||
                     type == GPS_TYPE_UNICORE_MOVINGBASE_NMEA ||
+#endif
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+                    type == GPS_TYPE_QUECTEL_NMEA ||
 #endif
                     type == GPS_TYPE_ALLYSTAR) &&
                    AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 929ac4394b06de..bb33bda05e9870 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -112,6 +112,7 @@ class AP_GPS
         GPS_TYPE_UNICORE_NMEA = 24,
         GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
         GPS_TYPE_SBF_DUAL_ANTENNA = 26,
+        GPS_TYPE_QUECTEL_NMEA = 27,
 #if AP_SIM_GPS_ENABLED
         GPS_TYPE_SITL = 100,
 #endif
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 8d8c0db18501a4..6c37a38458e9d5 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -281,6 +281,28 @@ bool AP_GPS_NMEA::_have_new_message()
     }
 #endif // AP_GPS_NMEA_UNICORE_ENABLED
 
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    // Check for PQTM message timeouts
+    // Note: PQTMPVT only provides auxiliary data (undulation, VDOP, leap seconds)
+    // so we don't treat it as essential for GPS health
+    if (now - _last_PQTM_vel_ms > 500) {
+        if (_last_PQTM_vel_ms != 0) {
+            // we have lost PQTM velocity messages
+            state.have_vertical_velocity = false;
+            state.have_speed_accuracy = false;
+            _last_PQTM_vel_ms = 0;
+        }
+    }
+    if (now - _last_PQTM_acc_ms > 500) {
+        if (_last_PQTM_acc_ms != 0) {
+            // we have lost PQTM accuracy messages  
+            state.have_horizontal_accuracy = false;
+            state.have_vertical_accuracy = false;
+            _last_PQTM_acc_ms = 0;
+        }
+    }
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
+
     _last_fix_ms = now;
 
     _last_GGA_ms = 1;
@@ -508,6 +530,27 @@ bool AP_GPS_NMEA::_term_complete()
                 break;
             }
 #endif // AP_GPS_NMEA_UNICORE_ENABLED
+
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+            case _GPS_SENTENCE_PQTMVER: {
+                if (_have_pqtmver) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+                                  "NMEA Quectel %s %s",
+                                  _pqtmver.version,
+                                  _pqtmver.build_date);
+                }
+                break;
+            }
+            case _GPS_SENTENCE_PQTMEPE:
+                // Accuracy information already processed in parse function
+                break;
+            case _GPS_SENTENCE_PQTMVEL:
+                // Velocity information already processed in parse function
+                break;
+            case _GPS_SENTENCE_PQTMPVT:
+                // PVT information already processed in parse function
+                break;
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
             }
             // see if we got a good message
             return _have_new_message();
@@ -543,6 +586,26 @@ bool AP_GPS_NMEA::_term_complete()
             return false;
         }
 #endif
+
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+        // Check for Quectel PQTM messages
+        if (strcmp(_term, "PQTMVERNO") == 0 && _expect_pqtm) {
+            _sentence_type = _GPS_SENTENCE_PQTMVER;
+            return false;
+        }
+        if (strcmp(_term, "PQTMEPE") == 0 && _expect_pqtm) {
+            _sentence_type = _GPS_SENTENCE_PQTMEPE;
+            return false;
+        }
+        if (strcmp(_term, "PQTMVEL") == 0 && _expect_pqtm) {
+            _sentence_type = _GPS_SENTENCE_PQTMVEL;
+            return false;
+        }
+        if (strcmp(_term, "PQTMPVT") == 0 && _expect_pqtm) {
+            _sentence_type = _GPS_SENTENCE_PQTMPVT;
+            return false;
+        }
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
         /*
           The first two letters of the NMEA term are the talker
           ID. The most common is 'GP' but there are a bunch of others
@@ -670,6 +733,22 @@ bool AP_GPS_NMEA::_term_complete()
             break;
 #endif
 #endif
+
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+        // Quectel PQTM message parsing
+        case _GPS_SENTENCE_PQTMVER + 1 ... _GPS_SENTENCE_PQTMVER + 3: // PQTMVER message (3 data fields)
+            parse_pqtmver_field(_term_number, _term);
+            break;
+        case _GPS_SENTENCE_PQTMEPE + 1 ... _GPS_SENTENCE_PQTMEPE + 6: // PQTMEPE message (6 fields)
+            parse_pqtmepe_field(_term_number, _term);
+            break;
+        case _GPS_SENTENCE_PQTMVEL + 1 ... _GPS_SENTENCE_PQTMVEL + 11: // PQTMVEL message (11 fields)
+            parse_pqtmvel_field(_term_number, _term);
+            break;
+        case _GPS_SENTENCE_PQTMPVT + 1 ... _GPS_SENTENCE_PQTMPVT + 19: // PQTMPVT message (19 fields)
+            parse_pqtmpvt_field(_term_number, _term);
+            break;
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
         }
     }
 
@@ -794,6 +873,150 @@ void AP_GPS_NMEA::parse_versiona_field(uint16_t term_number, const char *term)
 }
 #endif // AP_GPS_NMEA_UNICORE_ENABLED
 
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+/*
+  Parse PQTM message fields for Quectel GNSS modules
+ */
+
+/*
+  parse PQTMVER field - Firmware Version Output
+  Example: $PQTMVER,1,MODULE,LG290P03AANR01A03S,2024/04/30,10:53:07*32
+ */
+void AP_GPS_NMEA::parse_pqtmver_field(uint16_t term_number, const char *term)
+{
+    auto &pv = _pqtmver;
+    
+    switch (term_number) {
+    case 1: // Version string (VerStr)
+        strncpy(pv.version, term, sizeof(pv.version)-1);
+        pv.version[sizeof(pv.version)-1] = 0;
+        break;
+    case 2: // Build date (BuildDate)
+        strncpy(pv.build_date, term, sizeof(pv.build_date)-1);
+        pv.build_date[sizeof(pv.build_date)-1] = 0;
+        break;
+    case 3: // Build time (BuildTime)
+        strncpy(pv.build_time, term, sizeof(pv.build_time)-1);
+        pv.build_time[sizeof(pv.build_time)-1] = 0;
+        _have_pqtmver = true;
+        break;
+    }
+}
+
+/*
+  parse PQTMEPE field - Estimated Position Error
+  Example: $PQTMEPE,2,1.000,1.000,1.000,1.414,1.732*52
+ */
+void AP_GPS_NMEA::parse_pqtmepe_field(uint16_t term_number, const char *term)
+{
+    auto &pe = _pqtmepe;
+    switch (term_number) {
+    case 2: // EPE North
+        pe.epe_north = atof(term);
+        break;
+    case 3: // EPE East
+        pe.epe_east = atof(term);
+        break;
+    case 4: // EPE Down
+        pe.epe_down = atof(term);
+        break;
+    case 5: // EPE 2D
+        pe.epe_2d = atof(term);
+        break;
+    case 6: // EPE 3D
+        pe.epe_3d = atof(term);
+        // Update state accuracy information
+        state.horizontal_accuracy = pe.epe_2d;
+        state.have_horizontal_accuracy = true;
+        state.vertical_accuracy = pe.epe_down;
+        state.have_vertical_accuracy = true;
+        _last_PQTM_acc_ms = AP_HAL::millis();
+        break;
+    }
+}
+
+/*
+  parse PQTMVEL field - Velocity Information
+  Example: $PQTMVEL,1,154512.100,1.251,2.452,1.245,2.752,3.021,180.512,0.124,0.254,0.250*67
+ */
+void AP_GPS_NMEA::parse_pqtmvel_field(uint16_t term_number, const char *term)
+{
+    auto &pv = _pqtmvel;
+    switch (term_number) {
+    case 2: // UTC time
+        pv.time_ms = _parse_decimal_100(term) * 10;
+        break;
+    case 3: // North velocity
+        pv.vel_NED.x = atof(term);
+        break;
+    case 4: // East velocity
+        pv.vel_NED.y = atof(term);
+        break;
+    case 5: // Down velocity
+        pv.vel_NED.z = atof(term);
+        break;
+    case 6: // Ground speed
+        pv.ground_speed = atof(term);
+        break;
+    case 7: // 3D speed
+        pv.speed_3d = atof(term);
+        break;
+    case 8: // Heading
+        pv.heading = atof(term);
+        break;
+    case 9: // Ground speed accuracy
+        pv.ground_speed_acc = atof(term);
+        break;
+    case 10: // 3D speed accuracy
+        pv.speed_acc = atof(term);
+        break;
+    case 11: // Heading accuracy
+        pv.heading_acc = atof(term);
+        // Update state with velocity information
+        state.velocity = pv.vel_NED;
+        state.have_vertical_velocity = true;
+        state.ground_speed = pv.ground_speed;
+        state.ground_course = pv.heading;
+        state.speed_accuracy = pv.speed_acc;
+        state.have_speed_accuracy = true;
+        _last_PQTM_vel_ms = AP_HAL::millis();
+        break;
+    }
+}
+
+/*
+  parse PQTMPVT field - Only auxiliary data not available in standard NMEA
+  Example: $PQTMPVT,1,31075000,20221225,083737.000,,3,09,18,31.12738291,117.26372910,34.212,5.267,3.212,2.928,0.238,4.346,34.12,2.16,4.38*51
+ */
+void AP_GPS_NMEA::parse_pqtmpvt_field(uint16_t term_number, const char *term)
+{
+    auto &pp = _pqtmpvt;
+    switch (term_number) {
+    case 2: // Time of week
+        pp.tow = atol(term);
+        break;
+    case 8: // Leap seconds
+        if (strlen(term) > 0) {
+            pp.leap_seconds = atol(term);
+        }
+        break;
+    case 12: // Geoidal separation
+        if (strlen(term) > 0) {
+            pp.sep = atof(term);
+            state.undulation = -pp.sep;
+            state.have_undulation = true;
+        }
+        break;
+    case 19: // PDOP
+        if (strlen(term) > 0) {
+            pp.pdop = atof(term);
+            state.vdop = pp.pdop * 100;
+        }
+        break;
+    }
+}
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
+
 /*
   detect a NMEA GPS. Adds one byte, and returns true if the stream
   matches a NMEA string
@@ -841,6 +1064,7 @@ void AP_GPS_NMEA::send_config(void)
     const auto type = get_type();
     _expect_agrica = (type == AP_GPS::GPS_TYPE_UNICORE_NMEA ||
                       type == AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA);
+    _expect_pqtm = (type == AP_GPS::GPS_TYPE_QUECTEL_NMEA);
     if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
         // not doing auto-config
         return;
@@ -902,6 +1126,49 @@ void AP_GPS_NMEA::send_config(void)
                     unsigned(rate_hz), unsigned(rate_ms));
         break;
 
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    case AP_GPS::GPS_TYPE_QUECTEL_NMEA: {
+        // Configure Quectel GNSS for PQTM message output
+        // Set fix rate (1Hz to 10Hz typically)
+
+        nmea_printf(port, "$PQTMCFGFIXRATE,W,%u", rate_ms);
+        
+        // It seems per the documentation that only 1Hz is supported for non-RTCM messages
+        // // Configure standard NMEA message output rates
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,GGA,%u", rate_hz);
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,RMC,%u", rate_hz);
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,VTG,%u", rate_hz);
+
+        // // Configure PQTM proprietary message output rates
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMEPE,%u,2", rate_hz);
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMVEL,%u,1", rate_hz);
+        // nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMPVT,%u,1", rate_hz);
+
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,GGA,1");
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,RMC,1");
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,VTG,1");
+        
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMVEL,1,1");
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMEPE,1,2");
+        nmea_printf(port, "$PQTMCFGMSGRATE,W,PQTMPVT,1,1");
+        
+        if (!_have_pqtmver) {
+            nmea_printf(port, "$PQTMVERNO");
+#if AP_GPS_DEBUG_LOGGING_ENABLED
+            nmea_printf(port, "$PQTMDEBUGON");
+#else
+            nmea_printf(port, "$PQTMDEBUGOFF");
+#endif
+            if (gps._save_config) {
+                // Save configuration for next startup
+                nmea_printf(port, "$PQTMSAVEPAR");
+            }
+        }
+    
+        break;
+    }
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
+
     default:
         break;
     }
@@ -933,6 +1200,11 @@ bool AP_GPS_NMEA::is_healthy(void) const
         // we should get vertical velocity and accuracy from PHD
         return _last_vvelocity_ms != 0 && _last_vaccuracy_ms != 0;
 
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    case AP_GPS::GPS_TYPE_QUECTEL_NMEA:
+        // we should be getting PQTM messages for velocity and accuracy
+        return _last_PQTM_vel_ms != 0 && _last_PQTM_acc_ms != 0;
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
     default:
         break;
     }
@@ -970,6 +1242,14 @@ void AP_GPS_NMEA::Write_AP_Logger_Log_Startup_messages() const
                                     _versiona.build_date);
     }
 #endif
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    if (_have_pqtmver) {
+        AP::logger().Write_MessageF("NMEA %u Quectel %s %s",
+                                    state.instance+1,
+                                    _pqtmver.version,
+                                    _pqtmver.build_date);
+    }
+#endif // AP_GPS_NMEA_QUECTEL_ENABLED
 }
 #endif
 
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 3109600b4ef8a2..44bfb2aef0d3df 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -89,6 +89,10 @@ class AP_GPS_NMEA : public AP_GPS_Backend
         _GPS_SENTENCE_AGRICA = 193, // extension for Unicore, 65 fields
         _GPS_SENTENCE_VERSIONA = 270, // extension for Unicore, version, 10 fields
         _GPS_SENTENCE_UNIHEADINGA = 290, // extension for Unicore, uniheadinga, 20 fields
+        _GPS_SENTENCE_PQTMVER = 320, // extension for Quectel, version info
+        _GPS_SENTENCE_PQTMEPE = 330, // extension for Quectel, estimated position error
+        _GPS_SENTENCE_PQTMVEL = 340, // extension for Quectel, velocity information
+        _GPS_SENTENCE_PQTMPVT = 360, // extension for Quectel, PVT result
         _GPS_SENTENCE_OTHER = 0
     };
 
@@ -146,6 +150,15 @@ class AP_GPS_NMEA : public AP_GPS_Backend
 #endif
 #endif
 
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    /*
+      parse PQTM message fields
+     */
+    void parse_pqtmver_field(uint16_t term_number, const char *term);
+    void parse_pqtmepe_field(uint16_t term_number, const char *term);
+    void parse_pqtmvel_field(uint16_t term_number, const char *term);
+    void parse_pqtmpvt_field(uint16_t term_number, const char *term);
+#endif
 
     uint8_t _parity;                                                    ///< NMEA message checksum accumulator
     uint32_t _crc32;                                            ///< CRC for unicore messages
@@ -257,6 +270,52 @@ class AP_GPS_NMEA : public AP_GPS_Backend
     } _uniheadinga;
 #endif
 #endif // AP_GPS_NMEA_UNICORE_ENABLED
+
+#if AP_GPS_NMEA_QUECTEL_ENABLED
+    /*
+      Quectel PQTM message parsing structures
+     */
+    // PQTMVER parsing
+    struct {
+        char version[30];        // Version string (e.g., "LG290P03AANR01A05S")
+        char build_date[20];     // Build date (e.g., "2025/05/08")
+        char build_time[20];     // Build time (e.g., "16:23:00")
+    } _pqtmver;
+    bool _have_pqtmver;
+
+    // PQTMEPE parsing - Estimated Position Error
+    struct {
+        float epe_north;
+        float epe_east;
+        float epe_down;
+        float epe_2d;
+        float epe_3d;
+    } _pqtmepe;
+
+    // PQTMVEL parsing - Velocity Information
+    struct {
+        uint32_t time_ms;
+        Vector3f vel_NED;
+        float ground_speed;
+        float speed_3d;
+        float heading;
+        float ground_speed_acc;
+        float speed_acc;
+        float heading_acc;
+    } _pqtmvel;
+
+    // PQTMPVT parsing - PVT Result
+    struct {
+        uint32_t tow;           // Time of week
+        int32_t leap_seconds;   // Leap seconds
+        float sep;              // Geoidal separation (undulation)
+        float pdop;             // PDOP (used for VDOP)
+    } _pqtmpvt;
+
+    uint32_t _last_PQTM_vel_ms;
+    uint32_t _last_PQTM_acc_ms;
+#endif  // AP_GPS_NMEA_QUECTEL_ENABLED
+    bool _expect_pqtm;
     bool _expect_agrica;
 
     // last time we sent type specific config strings
diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h
index ad6b5364a29dfd..66c005c5334570 100644
--- a/libraries/AP_GPS/AP_GPS_config.h
+++ b/libraries/AP_GPS/AP_GPS_config.h
@@ -73,6 +73,10 @@
   #define AP_GPS_NMEA_UNICORE_ENABLED AP_GPS_NMEA_ENABLED
 #endif
 
+#ifndef AP_GPS_NMEA_QUECTEL_ENABLED
+  #define AP_GPS_NMEA_QUECTEL_ENABLED AP_GPS_NMEA_ENABLED
+#endif
+
 #ifndef AP_GPS_NOVA_ENABLED
   #define AP_GPS_NOVA_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED
 #endif

From 2b342362ca4da1a7c4e16804f2dddd10b410ca59 Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 16:16:51 +0200
Subject: [PATCH 02/19] quectel nmea - apply peterbarker suggestions

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 9 +++------
 libraries/AP_GPS/AP_GPS_NMEA.h   | 3 ++-
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 6c37a38458e9d5..43b398d9294055 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -888,16 +888,13 @@ void AP_GPS_NMEA::parse_pqtmver_field(uint16_t term_number, const char *term)
     
     switch (term_number) {
     case 1: // Version string (VerStr)
-        strncpy(pv.version, term, sizeof(pv.version)-1);
-        pv.version[sizeof(pv.version)-1] = 0;
+        strncpy(pv.version, term, sizeof(pv.version));
         break;
     case 2: // Build date (BuildDate)
-        strncpy(pv.build_date, term, sizeof(pv.build_date)-1);
-        pv.build_date[sizeof(pv.build_date)-1] = 0;
+        strncpy(pv.build_date, term, sizeof(pv.build_date));
         break;
     case 3: // Build time (BuildTime)
-        strncpy(pv.build_time, term, sizeof(pv.build_time)-1);
-        pv.build_time[sizeof(pv.build_time)-1] = 0;
+        strncpy(pv.build_time, term, sizeof(pv.build_time));
         _have_pqtmver = true;
         break;
     }
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 44bfb2aef0d3df..a4c2b46272c9dd 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -314,8 +314,9 @@ class AP_GPS_NMEA : public AP_GPS_Backend
 
     uint32_t _last_PQTM_vel_ms;
     uint32_t _last_PQTM_acc_ms;
-#endif  // AP_GPS_NMEA_QUECTEL_ENABLED
     bool _expect_pqtm;
+#endif  // AP_GPS_NMEA_QUECTEL_ENABLED
+
     bool _expect_agrica;
 
     // last time we sent type specific config strings

From acafa32b7c51929fb1033b2038ae79ed94cff13e Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 17:53:29 +0200
Subject: [PATCH 03/19] quectel add - pqmtar heading

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 128 +++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS_NMEA.h   |  22 ++++++
 2 files changed, 150 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 576f81f8cbd171..edb0daa9c654f3 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -605,6 +605,10 @@ bool AP_GPS_NMEA::_term_complete()
             _sentence_type = _GPS_SENTENCE_PQTMPVT;
             return false;
         }
+        if (strcmp(_term, "PQTMTAR") == 0 && _expect_pqtm) {
+            _sentence_type = _GPS_SENTENCE_PQTMTAR;
+            return false;
+        }
 #endif // AP_GPS_NMEA_QUECTEL_ENABLED
         /*
           The first two letters of the NMEA term are the talker
@@ -748,6 +752,11 @@ bool AP_GPS_NMEA::_term_complete()
         case _GPS_SENTENCE_PQTMPVT + 1 ... _GPS_SENTENCE_PQTMPVT + 19: // PQTMPVT message (19 fields)
             parse_pqtmpvt_field(_term_number, _term);
             break;
+        #if GPS_MOVING_BASELINE
+        case _GPS_SENTENCE_PQTMTAR + 1 ... _GPS_SENTENCE_PQTMTAR + 12: // PQTMTAR message (12 fields)
+            parse_pqtmtar_field(_term_number, _term);
+            break;
+        #endif // GPS_MOVING_BASELINE
 #endif // AP_GPS_NMEA_QUECTEL_ENABLED
         }
     }
@@ -1012,6 +1021,125 @@ void AP_GPS_NMEA::parse_pqtmpvt_field(uint16_t term_number, const char *term)
         break;
     }
 }
+
+#if GPS_MOVING_BASELINE
+
+/*
+  parse PQTMTAR field - Outputs the time and attitude. The attitude computation in this message is computed from the two-antenna system.
+  Only the LG580P supports this message.
+
+  Examples:
+  - with RTK fix:
+    $PQTMTAR,1,215951.600,5,,0.206,-11.841884,,234.657328,89.989998,,177.466919,20*49
+  - without RTK fix:
+    $PQTMTAR,1,215857.400,1,,0.000,,,,,,,21*64
+
+
+ Format:
+ $PQTMTAR,<MsgVer>,<Time>,<Quality>,<Res>,<Length>,<Pitch>,<Roll>,<Heading>,<Acc_Pitch>,<Acc_Roll>,<Acc_Heading>,<UsedSV>*<Checksum><CR><LF>
+
+>>> list(enumerate("""$PQTMTAR,<MsgVer>,<Time>,<Quality>,<Res>,<Length>,<Pitch>,<Roll>,<Heading>,<Acc_Pitch>,<Acc_Roll>,<Acc_Heading>,<UsedSV>*<Checksum><CR><LF>""".split(',')))
+
+[(0, '$PQTMTAR'), (1, '<MsgVer>'), (2, '<Time>'), (3, '<Quality>'), (4, '<Res>'), (5, '<Length>'), (6, '<Pitch>'), (7, '<Roll>'), (8, '<Heading>'), 
+(9, '<Acc_Pitch>'), (10, '<Acc_Roll>'), (11, '<Acc_Heading>'), (12, '<UsedSV>*<Checksum><CR><LF>')]
+ */
+void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
+{
+    // FIXME
+    auto &ph = _pqmtarheading;
+
+    switch (term_number) {
+    case 2: // UTC Time (hhmmss.ddd)
+        // FIXME: is this loosing accuracy - the last digit?
+        ph.time_ms = _parse_decimal_100(term) * 10;
+        break;
+    case 3: // GPS Heading Quality
+        // 0 = Fix not available or invalid
+        // 1 = GPS SPS Mode, fix valid
+        // 2 = Differential GPS, SPS Mode, or Satellite Based Augmentation System (SBAS), fix valid
+        // 3 = GPS PPS Mode, fix valid
+        // 4 = Fix Heading. System used in RTK mode with fixed integers
+        // 5 = Float Heading. Satellite system used in RTK mode, floating integers
+        ph.quality = atoi(term);
+        break;
+    // case 4: // Reserved
+    case 5: // Baseline length (meters)
+        ph.baseline_length = atof(term);
+        break;
+    case 6: // Pitch Angle (degrees, -90.0 -90.0)
+        ph.pitch = atof(term);
+        break;
+    case 7: // Roll Angle (degrees, -180.0 -180.0)
+        ph.roll = atof(term);
+        break;
+    case 8: // Heading (degrees, 0.0-360.0)
+        ph.heading = atof(term);
+        break;
+    case 9: // Pitch Accuracy (degrees)
+        ph.pitch_acc = atof(term);
+        break;
+    case 10: // Roll Accuracy (degrees)
+        ph.roll_acc = atof(term);
+        break;
+    case 11: // Heading Accuracy (degrees)
+        ph.heading_acc = atof(term);
+        // FIXME: Update state with heading information
+        // state.velocity = ph.vel_NED;
+        // state.have_vertical_velocity = true;
+        // state.ground_speed = ph.ground_speed;
+        // state.ground_course = ph.heading;
+        // state.speed_accuracy = ph.speed_acc;
+        // state.have_speed_accuracy = true;
+        // _last_PQTM_vel_ms = AP_HAL::millis();
+        break;
+    case 12:  // 12 - <UsedSV>	Satellites Number used for heading solution
+        ph.used_sv = atoi(term);
+        #if HAL_LOGGING_ENABLED
+        // FIXME: write to log
+        // ph.time_ms,
+        // ph.quality,
+        // ph.baseline_length,
+        // ph.pitch,
+        // ph.roll,
+        // ph.heading,
+        // ph.pitch_acc,
+        // ph.roll_acc,
+        // ph.heading_acc,
+        // ph.used_sv
+
+        AP::logger().Write_MessageF("head %u q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
+                                    state.instance+1,
+                                    ph.time_ms,
+                                    ph.quality,
+                                    ph.baseline_length,
+                                    ph.pitch,
+                                    ph.roll,
+                                    ph.heading
+
+                                    // ph.pitch_acc,
+                                    // ph.roll_acc,
+                                    // ph.heading_acc,
+                                    // ph.used_sv
+        );
+
+        AP::logger().Write_MessageF("head acc:%.1f %.1f %.1f sv:%u",
+                                    state.instance+1,
+                                    //ph.time_ms,
+                                    // ph.quality,
+                                    // ph.baseline_length,
+                                    // ph.pitch,
+                                    // ph.roll,
+                                    // ph.heading,
+                                    ph.pitch_acc,
+                                    ph.roll_acc,
+                                    ph.heading_acc,
+                                    ph.used_sv);
+        #endif // HAL_LOGGING_ENABLED
+        break;
+    }
+}
+#endif // GPS_MOVING_BASELINE
+
 #endif // AP_GPS_NMEA_QUECTEL_ENABLED
 
 /*
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index a4c2b46272c9dd..f8220f809cd597 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -93,6 +93,7 @@ class AP_GPS_NMEA : public AP_GPS_Backend
         _GPS_SENTENCE_PQTMEPE = 330, // extension for Quectel, estimated position error
         _GPS_SENTENCE_PQTMVEL = 340, // extension for Quectel, velocity information
         _GPS_SENTENCE_PQTMPVT = 360, // extension for Quectel, PVT result
+        _GPS_SENTENCE_PQTMTAR = 380, // extension for Quectel, heading info, 12 fields
         _GPS_SENTENCE_OTHER = 0
     };
 
@@ -158,6 +159,9 @@ class AP_GPS_NMEA : public AP_GPS_Backend
     void parse_pqtmepe_field(uint16_t term_number, const char *term);
     void parse_pqtmvel_field(uint16_t term_number, const char *term);
     void parse_pqtmpvt_field(uint16_t term_number, const char *term);
+#if GPS_MOVING_BASELINE
+    void parse_pqtmtar_field(uint16_t term_number, const char *term);
+#endif // GPS_MOVING_BASELINE
 #endif
 
     uint8_t _parity;                                                    ///< NMEA message checksum accumulator
@@ -312,6 +316,24 @@ class AP_GPS_NMEA : public AP_GPS_Backend
         float pdop;             // PDOP (used for VDOP)
     } _pqtmpvt;
 
+    #if GPS_MOVING_BASELINE
+    // quectel PQTMTAR parsing
+    struct {
+        uint32_t time_ms;
+        int quality;
+        float baseline_length;
+        float heading;
+        float pitch;
+        float roll;
+        float heading_acc;
+        float pitch_acc;
+        float roll_acc;
+        int used_sv;
+        // fixme - is this needed?
+        // float heading_sd;
+    } _pqmtarheading;
+    #endif // GPS_MOVING_BASELINE
+
     uint32_t _last_PQTM_vel_ms;
     uint32_t _last_PQTM_acc_ms;
     bool _expect_pqtm;

From 2e24e8c0ce2401c8137e522beedaeaadd46ca48a Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 17:53:50 +0200
Subject: [PATCH 04/19] temp upd mamba hwdef

---
 libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
index 035cac5bbbfff3..7afd9737b8918c 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
@@ -34,3 +34,7 @@ BARO SPL06 I2C:0:0x76
 define AP_BARO_SPL06_ENABLED 1
 
 define DEFAULT_NTF_LED_TYPES 257
+
+undef GPS_MOVING_BASELINE AP_GPS_NMEA_QUECTEL_ENABLED
+define GPS_MOVING_BASELINE 1
+define AP_GPS_NMEA_QUECTEL_ENABLED 1
\ No newline at end of file

From d31cfa91096d9e60bb20e800fedb69df62b4b2b5 Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 18:48:35 +0200
Subject: [PATCH 05/19] Add basic NMEA PPS

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp              | 26 ++++++++++++++++++-
 libraries/AP_GPS/AP_GPS_NMEA.h                |  6 +++++
 .../hwdef/MambaF405-2022/hwdef.dat            | 11 +++++++-
 3 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index edb0daa9c654f3..2c2af343d43d02 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -1190,6 +1190,21 @@ void AP_GPS_NMEA::send_config(void)
     _expect_agrica = (type == AP_GPS::GPS_TYPE_UNICORE_NMEA ||
                       type == AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA);
     _expect_pqtm = (type == AP_GPS::GPS_TYPE_QUECTEL_NMEA);
+
+    #ifdef HAL_GPIO_PPS
+        // FIXME: is there a better place to do this?
+
+        // FIXME: should it be rising or falling edge? Make configurable from ardupilot side?
+        // Quectelspecific - PQTMCFGPPS on quectel_lg290p03lgx80p03_gnss_protocol_specification_v1-1.pdf 
+        // $PQTMCFGPPS,W,<Index>,<Enable>,<Duration>,<Mode>,<Polarity>,<Reserved>*<Checksum><CR><LF>
+        // Pulse polarity. 0 = Low 1 = High
+        // depend on Polarity: 
+        // POSITIVE: PPS rising edge is effective.
+        // NEGATIVE: PPS falling edge is valid
+        // INTERRUPT_RISING or INTERRUPT_FALLING
+        hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_NMEA::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_RISING);
+    #endif // HAL_GPIO_PPS
+
     if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
         // not doing auto-config
         return;
@@ -1376,6 +1391,15 @@ void AP_GPS_NMEA::Write_AP_Logger_Log_Startup_messages() const
     }
 #endif // AP_GPS_NMEA_QUECTEL_ENABLED
 }
-#endif
+#endif // HAL_LOGGING_ENABLED
+
+
+#ifdef HAL_GPIO_PPS
+void
+AP_GPS_NMEA::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
+{
+    _last_pps_time_us = AP_HAL::micros64();
+}
+#endif // HAL_GPIO_PPS
 
 #endif // AP_GPS_NMEA_ENABLED
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index f8220f809cd597..1d7e824eba55fe 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -344,6 +344,12 @@ class AP_GPS_NMEA : public AP_GPS_Backend
     // last time we sent type specific config strings
     uint32_t last_config_ms;
 
+    uint8_t _pps_freq = 1;
+#ifdef HAL_GPIO_PPS
+    void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
+    //void set_pps_desired_freq(uint8_t freq) override;
+#endif
+
     // send type specific config strings
     void send_config(void);
 };
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
index 7afd9737b8918c..6099d9c78e3e19 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
@@ -35,6 +35,15 @@ define AP_BARO_SPL06_ENABLED 1
 
 define DEFAULT_NTF_LED_TYPES 257
 
+
 undef GPS_MOVING_BASELINE AP_GPS_NMEA_QUECTEL_ENABLED
 define GPS_MOVING_BASELINE 1
-define AP_GPS_NMEA_QUECTEL_ENABLED 1
\ No newline at end of file
+define AP_GPS_NMEA_QUECTEL_ENABLED 1
+
+# use LED strip pin as GPS PPS input
+undef PB3
+PB3  GPS_PPS INPUT FLOATING GPIO(56)
+#define HAL_GPIO_PPS 56
+
+// for now we assume PPS is pre-configured with defaults
+//define CONFIGURE_PPS_PIN TRUE

From cf83c257a27f2c65266dec52f88622c368c831a2 Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 22:06:26 +0200
Subject: [PATCH 06/19] debug

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 103 +++++++++++++++++++++++--------
 libraries/AP_GPS/AP_GPS_NMEA.h   |   1 +
 2 files changed, 77 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 2c2af343d43d02..6b567bca7623ab 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -1094,6 +1094,54 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
         break;
     case 12:  // 12 - <UsedSV>	Satellites Number used for heading solution
         ph.used_sv = atoi(term);
+        // ratelimit output
+        const uint32_t now = AP_HAL::millis();
+        bool log_pqmtar = ph.quality >= 4; // only log when we have RTK fix
+        if ((now - _last_pqmtar_log_ms > 1000) && log_pqmtar) {
+            _last_pqmtar_log_ms = now;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+                    "H q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
+
+                    ph.quality,
+                    ph.baseline_length,
+                    ph.pitch,
+                    ph.roll,
+                    ph.heading
+                    );
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+                    "Q acc:%.1f %.1f %.1f",
+                    
+                    ph.pitch_acc,
+                    ph.roll_acc,
+                    ph.heading_acc);
+        }
+        // FIXME: should be quality=4 only
+        bool accept_heading_quality = ph.quality == 4; // only accept when we have RTK fix
+        if (accept_heading_quality) {
+            // FIXME: should use this only when quality = 4
+            // have a valid RTK fixed heading solution
+
+            // FIXME: some shared logic with UNIHEADINGA parsing...
+            state.relPosHeading = ph.heading;
+            state.relPosLength = ph.baseline_length;
+            const float alt_diff = ph.baseline_length * tanf(radians(-ph.pitch));
+            state.relPosD = alt_diff;
+            state.relposheading_ts = now;
+            if (calculate_moving_base_yaw(ph.heading, ph.baseline_length, alt_diff)) {
+                state.have_gps_yaw_accuracy = true;
+                state.gps_yaw_accuracy = ph.heading_acc;
+                _last_yaw_ms = now;
+            }
+            state.gps_yaw_configured = true;
+            state.have_gps_yaw_accuracy = true;
+            state.gps_yaw_accuracy = ph.heading_acc;
+        }
+        if (!(accept_heading_quality) || _last_yaw_ms + 500 < now) {
+            // lost RTK fix
+            state.have_gps_yaw = false;
+            state.have_gps_yaw_accuracy = false;
+        }
+
         #if HAL_LOGGING_ENABLED
         // FIXME: write to log
         // ph.time_ms,
@@ -1107,33 +1155,34 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
         // ph.heading_acc,
         // ph.used_sv
 
-        AP::logger().Write_MessageF("head %u q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
-                                    state.instance+1,
-                                    ph.time_ms,
-                                    ph.quality,
-                                    ph.baseline_length,
-                                    ph.pitch,
-                                    ph.roll,
-                                    ph.heading
-
-                                    // ph.pitch_acc,
-                                    // ph.roll_acc,
-                                    // ph.heading_acc,
-                                    // ph.used_sv
-        );
-
-        AP::logger().Write_MessageF("head acc:%.1f %.1f %.1f sv:%u",
-                                    state.instance+1,
-                                    //ph.time_ms,
-                                    // ph.quality,
-                                    // ph.baseline_length,
-                                    // ph.pitch,
-                                    // ph.roll,
-                                    // ph.heading,
-                                    ph.pitch_acc,
-                                    ph.roll_acc,
-                                    ph.heading_acc,
-                                    ph.used_sv);
+
+        // AP::logger().Write_MessageF("head %u q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
+        //                             state.instance+1,
+        //                             ph.time_ms,
+        //                             ph.quality,
+        //                             ph.baseline_length,
+        //                             ph.pitch,
+        //                             ph.roll,
+        //                             ph.heading
+
+        //                             // ph.pitch_acc,
+        //                             // ph.roll_acc,
+        //                             // ph.heading_acc,
+        //                             // ph.used_sv
+        // );
+
+        // AP::logger().Write_MessageF("head acc:%.1f %.1f %.1f sv:%u",
+        //                             state.instance+1,
+        //                             //ph.time_ms,
+        //                             // ph.quality,
+        //                             // ph.baseline_length,
+        //                             // ph.pitch,
+        //                             // ph.roll,
+        //                             // ph.heading,
+        //                             ph.pitch_acc,
+        //                             ph.roll_acc,
+        //                             ph.heading_acc,
+        //                             ph.used_sv);
         #endif // HAL_LOGGING_ENABLED
         break;
     }
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 1d7e824eba55fe..19bf16070b0b85 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -343,6 +343,7 @@ class AP_GPS_NMEA : public AP_GPS_Backend
 
     // last time we sent type specific config strings
     uint32_t last_config_ms;
+    uint32_t _last_pqmtar_log_ms;
 
     uint8_t _pps_freq = 1;
 #ifdef HAL_GPIO_PPS

From 3b125faf56e848a0ce6b46ca539c815a861fbadd Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 1 Feb 2026 22:06:36 +0200
Subject: [PATCH 07/19] mamba

---
 libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
index 6099d9c78e3e19..293246674c9aaa 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
@@ -43,7 +43,9 @@ define AP_GPS_NMEA_QUECTEL_ENABLED 1
 # use LED strip pin as GPS PPS input
 undef PB3
 PB3  GPS_PPS INPUT FLOATING GPIO(56)
-#define HAL_GPIO_PPS 56
+define HAL_GPIO_PPS 56
 
 // for now we assume PPS is pre-configured with defaults
 //define CONFIGURE_PPS_PIN TRUE
+
+//define _AP_GPS_DEBUG_LOGGING_ENABLED 0
\ No newline at end of file

From bbcba2a10a62feb34a73133f7cdee016023f0c1a Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 8 Feb 2026 23:39:40 +0200
Subject: [PATCH 08/19] nmea log PPS to bbox

---
 libraries/AP_GPS/AP_GPS.cpp      | 42 ++++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS.h        |  2 ++
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 15 +++++++++++-
 libraries/AP_GPS/LogStructure.h  |  1 +
 4 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 0682a80157755b..37ecea118aeb26 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -1953,6 +1953,48 @@ void AP_GPS::Write_GPS(uint8_t i)
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }
+
+void AP_GPS::Write_PPS(uint64_t interrupt_time_us, uint64_t earlier_interrupt_time_us)
+{
+
+    const uint64_t USEC_PER_SEC = 1000000ULL;
+    const float clock_drift_percent = (((float)interrupt_time_us - (float)earlier_interrupt_time_us) - (float)USEC_PER_SEC) * 100.0f / ((float)USEC_PER_SEC);
+
+    // FIXME: should it be dependent on GPS instance? but makes little sense
+    uint8_t instance = 0; // FIXME?
+
+    const GPS_State &istate = state[instance];
+
+    // it makes no sense to report PPS if there's no GPS FIX
+    if ((istate.last_gps_time_ms == 0 && istate.last_corrected_gps_time_us == 0) || istate.time_week == 0) {
+        return;
+    }
+    uint64_t _last_gps_systime_us = istate.last_gps_time_ms * 1000ULL; ///< the system time we got the last GPS timestamp, microseconds
+
+    // FIXME: check if fix still exists!?
+    uint64_t last_gps_fix_time_us = istate_time_to_epoch_ms(istate.time_week, istate.time_week_ms) * 1000ULL; // last GPS time
+
+    // GPS UTC time when the GPIO interrupt was triggered
+	// Last UTC time received from the GPS + elapsed time to the PPS interrupt
+
+    // FIXME: I possibly don't understand what this correction is used for? (based on PX4 PPSCpature driver)
+    // oh it's probably needed in case the GPS time was not received but PPS still fired?
+	uint64_t gps_utc_time_us = last_gps_fix_time_us + (interrupt_time_us - _last_gps_systime_us);
+
+	// (For ubx F9P and NMEA Quectel LG580P) The rising edge of the PPS pulse is aligned to the top of second GPS time base.
+	// So, remove the fraction of second and shift to the next second. The interrupt is triggered
+	// before the matching timestamp is received via a UART message, which means the last received GPS time is always
+	// behind.
+	const uint64_t corrected_utc_time_us = gps_utc_time_us - (gps_utc_time_us % USEC_PER_SEC) + USEC_PER_SEC;
+
+    struct log_PPS pkt {
+        LOG_PACKET_HEADER_INIT(LOG_PPS_MSG),
+        time_us       : interrupt_time_us,
+        utc_time_ms   : corrected_utc_time_us / 1000ULL,
+        clock_drift_percent : clock_drift_percent
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
 #endif
 
 bool AP_GPS::is_rtk_base(uint8_t instance) const
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 8a1b04e1c86615..168a4c05e17cb1 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -792,6 +792,8 @@ class AP_GPS
     // logging support
     void Write_GPS(uint8_t instance);
 
+    void Write_PPS(uint64_t interrupt_time_us, uint64_t earlier_interrupt_time_us);
+
 #if AP_GPS_RTCM_DECODE_ENABLED
     /*
       per mavlink channel RTCM decoder, enabled with RTCM decode
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 6b567bca7623ab..720bf51635821b 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -1447,7 +1447,20 @@ void AP_GPS_NMEA::Write_AP_Logger_Log_Startup_messages() const
 void
 AP_GPS_NMEA::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
 {
-    _last_pps_time_us = AP_HAL::micros64();
+    uint64_t now_us = AP_HAL::micros64();
+    uint64_t earlier_pps_time_us = _last_pps_time_us;
+    _last_pps_time_us = now_us;
+
+    // FIXME: this is possibly rather nasty, but let's keep things simple for now
+    AP_GPS::get_singleton()->Write_PPS(now_us, earlier_pps_time_us);
+
+    // Onlt temporarily for debugging
+    // uint64_t pps_interval_us = now_us - earlier_pps_time_us;
+    // int64_t pps_drift_us = (int64_t)((int64_t)pps_interval_us - 1000000); // expected interval is 1 second
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+    //             "PPS time drift=%lld us, at now=%llu us",
+    //             pps_drift_us, now_us);
+    // _last_pps_time_us = AP_HAL::micros64();
 }
 #endif // HAL_GPIO_PPS
 
diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
index 9296ec5f072012..97c8bc1ea5d3ed 100644
--- a/libraries/AP_GPS/LogStructure.h
+++ b/libraries/AP_GPS/LogStructure.h
@@ -6,6 +6,7 @@
 #define LOG_IDS_FROM_GPS                        \
     LOG_GPS_MSG,                                \
     LOG_GPA_MSG,                                \
+    LOG_PPS_MSG,                                \
     LOG_GPS_RAW_MSG,                            \
     LOG_GPS_RAWH_MSG,                           \
     LOG_GPS_RAWS_MSG,                           \

From dd764d9f6b2d7255e0f9da0c2f7a363d8a081bfd Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 8 Feb 2026 23:40:55 +0200
Subject: [PATCH 09/19] nmea log PPS to bbox - add msgs for PPS and GPHDG

---
 libraries/AP_GPS/LogStructure.h | 54 +++++++++++++++++++++++++++++++++
 1 file changed, 54 insertions(+)

diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
index 97c8bc1ea5d3ed..7b10e0eaceb6d0 100644
--- a/libraries/AP_GPS/LogStructure.h
+++ b/libraries/AP_GPS/LogStructure.h
@@ -10,6 +10,7 @@
     LOG_GPS_RAW_MSG,                            \
     LOG_GPS_RAWH_MSG,                           \
     LOG_GPS_RAWS_MSG,                           \
+    LOG_GPS_Heading_MSG,                        \
     LOG_GPS_UBX1_MSG,                           \
     LOG_GPS_UBX2_MSG,                           \
     LOG_IDS_FROM_GPS_SBP
@@ -84,6 +85,52 @@ struct PACKED log_GPA {
     uint16_t rtcm_fragments_discarded;
 };
 
+// @LoggerMessage: PPS
+// @Description: GPS PPS (pulse per second) information
+// @Field: TimeUS: Time since system startup
+// @Field: UTCTimeMS: GPS UTC time in milliseconds
+// @Field: ClockDriftPercent: PPS clock drift over last second (system time / 1 GPS second) * 100
+struct PACKED log_PPS {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint64_t utc_time_ms;
+    float clock_drift_percent;
+};
+
+/*
+  NMEA Quectel logging
+  From $PQTMTAR,<MsgVer>,<Time>,<Quality>,<Res>,<Length>,<Pitch>,<Roll>,<Heading>,<Acc_Pitch>,<Acc_Roll>,<Acc_Heading>,<UsedSV>*<Checksum><CR><LF>
+ */
+
+// @LoggerMessage: GHDG
+// @Description: GPS heading information from NMEA Quectel LG580P
+// @Field: TimeUS: GPS time in microseconds (accurate to the millisecond)
+// @Field: I: GPS instance number
+// @Field: Q: GPS Heading Quality (0-5, only 4 is useful)
+// @Field: Baseline: Baseline length in meters
+// @Field: Pitch: Pitch angle in degrees
+// @Field: Roll: Roll angle in degrees
+// @Field: Hdg: Heading in degrees
+// @Field: PitchA: Pitch accuracy in degrees
+// @Field: RollA: Roll accuracy in degrees
+// @Field: HdgA: Heading accuracy in degrees
+// @Field: SV: Satellites Number used for heading solution
+struct PACKED log_GPS_Heading {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t  instance;
+    uint8_t quality; // only 4 is usefl
+    int16_t  baseline_length_m;
+    int16_t  pitch_deg;
+    int16_t  roll_deg;
+    int16_t  heading_deg;
+    int16_t  pitch_acc_deg;
+    int16_t  roll_acc_deg;
+    int16_t  heading_acc_deg;
+    //uint16_t used_sv;
+};
+// 4+2+2*7+2 = 25 bytes
+
 /*
   UBlox logging
  */
@@ -209,10 +256,17 @@ struct PACKED log_GPS_RAWS {
       "GPS",  "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
     { LOG_GPA_MSG,  sizeof(log_GPA), \
       "GPA",  "QBCCCCfBIHeHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,AEl,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CCB--" , true }, \
+    { LOG_PPS_MSG,  sizeof(log_PPS), \
+      "PPS",  "QQf", "TimeUS,UTCTimeMS,ClockDriftPercent", "ss%", "FC0" , true }, \
     { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
       "UBX1", "QBHBBHI",  "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------"  , true }, \
     { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \
       "UBX2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ", "s#----", "F-----" , true }, \
+    { LOG_GPS_Heading_MSG, sizeof(log_GPS_Heading), \
+      "GPHDG", \
+      "QBBccccccc", "TimeUS,I,Q,Baseline,Pitch,Roll,Hdg,PitchA,RollA,HdgA", \
+      "s#-mddhddh", \
+      "F--BBBBBBB" , true }, \
     { LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \
       "GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli", "ss-S-------", "FC-0-------" , true }, \
     { LOG_GPS_RAWH_MSG, sizeof(log_GPS_RAWH), \

From 49178c9360cbdabc2b321c0071e5b8da69f81f6e Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 8 Feb 2026 23:41:56 +0200
Subject: [PATCH 10/19] wip implement LG580P heading and log to bbox

---
 Tools/scripts/extract_features.py |  2 +
 libraries/AP_GPS/AP_GPS_NMEA.cpp  | 82 ++++++++++++++-----------------
 libraries/AP_GPS/AP_GPS_NMEA.h    |  5 +-
 3 files changed, 43 insertions(+), 46 deletions(-)

diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index 60dba7ed838993..6d81892f9a0ea7 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -86,6 +86,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmepe_field',),
             ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmvel_field',),
             ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmpvt_field',),
+            ('AP_GPS_NMEA_QUECTEL_ENABLED', r'AP_GPS_NMEA::parse_pqtmtar_field',),
+
             ('AP_GPS_{type}_ENABLED', r'AP_GPS_(?P<type>.*)::read\b',),
 
             ('AP_OPTICALFLOW_ENABLED', 'AP_OpticalFlow::AP_OpticalFlow',),
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index 720bf51635821b..b19d4012f48b28 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -1024,6 +1024,35 @@ void AP_GPS_NMEA::parse_pqtmpvt_field(uint16_t term_number, const char *term)
 
 #if GPS_MOVING_BASELINE
 
+#if HAL_LOGGING_ENABLED
+
+int16_t float_to_int16_100(float value)
+{
+    const float scale = 100.0f;
+    return (uint16_t)(value * scale);
+}
+void AP_GPS_NMEA::log_GPS_Heading_pqtmtar()
+{
+    auto &ph = _pqmtarheading;
+
+    struct log_GPS_Heading pkt {
+        LOG_PACKET_HEADER_INIT(LOG_GPS_Heading_MSG),
+        time_us                 : ph.time_ms * 1000, // convert from milliseconds to microseconds
+        instance                : 0U,  // FIXME !? should be logged from elsewhere?
+        quality                 : (uint8_t)ph.quality,
+        baseline_length_m       : float_to_int16_100(ph.baseline_length),
+        pitch_deg               : float_to_int16_100(ph.pitch),
+        roll_deg                : float_to_int16_100(ph.roll),
+        heading_deg             : float_to_int16_100(ph.heading),
+        pitch_acc_deg           : float_to_int16_100(ph.pitch_acc),
+        roll_acc_deg            : float_to_int16_100(ph.roll_acc),
+        heading_acc_deg         : float_to_int16_100(ph.heading_acc),
+        //used_sv                 : (uint16_t)ph.used_sv,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+#endif // HAL_LOGGING_ENABLED
+
 /*
   parse PQTMTAR field - Outputs the time and attitude. The attitude computation in this message is computed from the two-antenna system.
   Only the LG580P supports this message.
@@ -1096,12 +1125,12 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
         ph.used_sv = atoi(term);
         // ratelimit output
         const uint32_t now = AP_HAL::millis();
-        bool log_pqmtar = ph.quality >= 4; // only log when we have RTK fix
-        if ((now - _last_pqmtar_log_ms > 1000) && log_pqmtar) {
+        bool log_pqmtar = ph.quality == 4; // only log when we have RTK fix
+        bool log_each_30s = now - _last_pqmtar_log_ms > 30000;
+        if ((now - _last_pqmtar_log_ms > 5000) && (log_pqmtar || log_each_30s)) {
             _last_pqmtar_log_ms = now;
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,
-                    "H q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
-
+                    "H q:%d b:%.1f p:%.1f r:%.1f h:%.1f",
                     ph.quality,
                     ph.baseline_length,
                     ph.pitch,
@@ -1110,7 +1139,6 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
                     );
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,
                     "Q acc:%.1f %.1f %.1f",
-                    
                     ph.pitch_acc,
                     ph.roll_acc,
                     ph.heading_acc);
@@ -1143,47 +1171,11 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
         }
 
         #if HAL_LOGGING_ENABLED
-        // FIXME: write to log
-        // ph.time_ms,
-        // ph.quality,
-        // ph.baseline_length,
-        // ph.pitch,
-        // ph.roll,
-        // ph.heading,
-        // ph.pitch_acc,
-        // ph.roll_acc,
-        // ph.heading_acc,
-        // ph.used_sv
-
-
-        // AP::logger().Write_MessageF("head %u q:%u b:%.1f p:%.1f r:%.1f h:%.1f",
-        //                             state.instance+1,
-        //                             ph.time_ms,
-        //                             ph.quality,
-        //                             ph.baseline_length,
-        //                             ph.pitch,
-        //                             ph.roll,
-        //                             ph.heading
-
-        //                             // ph.pitch_acc,
-        //                             // ph.roll_acc,
-        //                             // ph.heading_acc,
-        //                             // ph.used_sv
-        // );
-
-        // AP::logger().Write_MessageF("head acc:%.1f %.1f %.1f sv:%u",
-        //                             state.instance+1,
-        //                             //ph.time_ms,
-        //                             // ph.quality,
-        //                             // ph.baseline_length,
-        //                             // ph.pitch,
-        //                             // ph.roll,
-        //                             // ph.heading,
-        //                             ph.pitch_acc,
-        //                             ph.roll_acc,
-        //                             ph.heading_acc,
-        //                             ph.used_sv);
+        // FIXME: enable this only if RAW logging enabled?
+        // FIXME: should we log only when we have RTK fix? quality >= 4?
+        log_GPS_Heading_pqtmtar();
         #endif // HAL_LOGGING_ENABLED
+
         break;
     }
 }
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 19bf16070b0b85..ea485c406556c8 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -332,6 +332,9 @@ class AP_GPS_NMEA : public AP_GPS_Backend
         // fixme - is this needed?
         // float heading_sd;
     } _pqmtarheading;
+    #if HAL_LOGGING_ENABLED
+    void log_GPS_Heading_pqtmtar();
+    #endif // HAL_LOGGING_ENABLED
     #endif // GPS_MOVING_BASELINE
 
     uint32_t _last_PQTM_vel_ms;
@@ -343,7 +346,7 @@ class AP_GPS_NMEA : public AP_GPS_Backend
 
     // last time we sent type specific config strings
     uint32_t last_config_ms;
-    uint32_t _last_pqmtar_log_ms;
+    uint32_t _last_pqmtar_log_ms = 0;
 
     uint8_t _pps_freq = 1;
 #ifdef HAL_GPIO_PPS

From 33031ba5bee3629dea57e334bfa16b7bc321c12b Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Sun, 8 Feb 2026 23:42:26 +0200
Subject: [PATCH 11/19] wip micoair lite pps and lg580p heading

---
 .../AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat     | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
index 24f80c471bfe83..f19a3c5ce2d7b0 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
@@ -152,3 +152,13 @@ define HAL_OSD_TYPE_DEFAULT 5
 
 # setup for BF migration
 define HAL_FRAME_TYPE 12
+
+undef GPS_MOVING_BASELINE AP_GPS_NMEA_QUECTEL_ENABLED
+define GPS_MOVING_BASELINE 1
+define AP_GPS_NMEA_QUECTEL_ENABLED 1
+
+#  undef and define PPS pin
+undef PD14
+# fixme - consider PULLDOWN
+PD14  GPS_PPS INPUT FLOATING GPIO(64) #M15
+define HAL_GPIO_PPS 64

From f0f8bdd01d3307672f4a3e79916730ed9b481f51 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Mon, 13 May 2024 17:35:43 -0600
Subject: [PATCH 12/19] Tools: scripts: add SCH16T to decode_devid.py

---
 Tools/scripts/decode_devid.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py
index 3a432403e4be50..c8730df8f4dfa1 100755
--- a/Tools/scripts/decode_devid.py
+++ b/Tools/scripts/decode_devid.py
@@ -110,6 +110,7 @@ def num(s):
     0x3B : "DEVTYPE_INS_ICM45686",
     0x3C : "DEVTYPE_INS_SCHA63T",
     0x3D : "DEVTYPE_INS_IIM42653",
+    0x3D : "DEVTYPE_INS_SCH16T",
 }
 
 baro_types = {

From 3175ba98db6a74ce8bcbcb70245fc0f580af60aa Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Mon, 13 May 2024 17:36:35 -0600
Subject: [PATCH 13/19] AP_InertialSensor: added driver for SCH16T

---
 Tools/scripts/decode_devid.py                 |   2 +-
 .../AP_InertialSensor/AP_InertialSensor.cpp   |   2 +
 .../AP_InertialSensor_Backend.h               |   1 +
 .../AP_InertialSensor_SCH16T.cpp              | 447 ++++++++++++++++++
 .../AP_InertialSensor_SCH16T.h                | 116 +++++
 5 files changed, 567 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
 create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h

diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py
index c8730df8f4dfa1..f9abe1a910df76 100755
--- a/Tools/scripts/decode_devid.py
+++ b/Tools/scripts/decode_devid.py
@@ -110,7 +110,7 @@ def num(s):
     0x3B : "DEVTYPE_INS_ICM45686",
     0x3C : "DEVTYPE_INS_SCHA63T",
     0x3D : "DEVTYPE_INS_IIM42653",
-    0x3D : "DEVTYPE_INS_SCH16T",
+    0x3E : "DEVTYPE_INS_SCH16T",
 }
 
 baro_types = {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 6c962e3b145c18..276a561f7fca8c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -38,6 +38,8 @@
 #include "AP_InertialSensor_Invensensev3.h"
 #include "AP_InertialSensor_NONE.h"
 #include "AP_InertialSensor_SCHA63T.h"
+#include "AP_InertialSensor_SCH16T.h"
+
 #include <AP_Scheduler/AP_Scheduler.h>
 
 /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 75267f6afa64d1..375275044ed90a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -152,6 +152,7 @@ class AP_InertialSensor_Backend
         DEVTYPE_INS_ICM45686 = 0x3B,
         DEVTYPE_INS_SCHA63T  = 0x3C,
         DEVTYPE_INS_IIM42653 = 0x3D,
+        DEVTYPE_INS_SCH16T   = 0x3E,
     };
 
 protected:
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
new file mode 100644
index 00000000000000..a04956571d3fd1
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -0,0 +1,447 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_Math/AP_Math.h>
+
+#include "AP_InertialSensor_SCH16T.h"
+// #include <GCS_MAVLink/GCS.h>
+
+#if defined(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1)
+#include <hal.h>
+#endif
+
+static constexpr uint16_t EOI = (1 << 1);               // End of Initialization
+static constexpr uint16_t EN_SENSOR = (1 << 0);         // Enable RATE and ACC measurement
+static constexpr uint16_t DRY_DRV_EN = (1 << 5);        // Enables Data ready function
+static constexpr uint16_t FILTER_68HZ = (0x0000);       // 68 Hz default filter
+static constexpr uint16_t FILTER_BYPASS = (0b0000000111111111);     // No filtering
+static constexpr uint16_t RATE_300DPS_1475HZ = 0b0001001011011011; // Gyro XYZ range 300 deg/s @ 1475Hz
+static constexpr uint16_t ACC12_8G_1475HZ = 0b0001001011011011;  // Acc XYZ range 8 G and 1475 update rate
+static constexpr uint16_t ACC3_26G = (0b000 << 0);
+static constexpr uint16_t SPI_SOFT_RESET = (0b1010);
+static constexpr uint32_t POWER_ON_TIME = 250000UL;
+
+// Data registers
+#define RATE_X1         0x01 // 20 bit
+#define RATE_Y1         0x02 // 20 bit
+#define RATE_Z1         0x03 // 20 bit
+#define ACC_X1          0x04 // 20 bit
+#define ACC_Y1          0x05 // 20 bit
+#define ACC_Z1          0x06 // 20 bit
+#define ACC_X3          0x07 // 20 bit
+#define ACC_Y3          0x08 // 20 bit
+#define ACC_Z3          0x09 // 20 bit
+#define RATE_X2         0x0A // 20 bit
+#define RATE_Y2         0x0B // 20 bit
+#define RATE_Z2         0x0C // 20 bit
+#define ACC_X2          0x0D // 20 bit
+#define ACC_Y2          0x0E // 20 bit
+#define ACC_Z2          0x0F // 20 bit
+#define TEMP            0x10 // 16 bit
+// Status registers
+#define STAT_SUM        0x14 // 16 bit
+#define STAT_SUM_SAT    0x15 // 16 bit
+#define STAT_COM        0x16 // 16 bit
+#define STAT_RATE_COM   0x17 // 16 bit
+#define STAT_RATE_X     0x18 // 16 bit
+#define STAT_RATE_Y     0x19 // 16 bit
+#define STAT_RATE_Z     0x1A // 16 bit
+#define STAT_ACC_X      0x1B // 16 bit
+#define STAT_ACC_Y      0x1C // 16 bit
+#define STAT_ACC_Z      0x1D // 16 bit
+// Control registers
+#define CTRL_FILT_RATE  0x25 // 9 bit
+#define CTRL_FILT_ACC12 0x26 // 9 bit
+#define CTRL_FILT_ACC3  0x27 // 9 bit
+#define CTRL_RATE       0x28 // 15 bit
+#define CTRL_ACC12      0x29 // 15 bit
+#define CTRL_ACC3       0x2A // 3 bit
+#define CTRL_USER_IF    0x33 // 16 bit
+#define CTRL_ST         0x34 // 13 bit
+#define CTRL_MODE       0x35 // 4 bit
+#define CTRL_RESET      0x36 // 4 bit
+// Misc registers
+#define ASIC_ID         0x3B // 12 bit
+#define COMP_ID         0x3C // 16 bit
+#define SN_ID1          0x3D // 16 bit
+#define SN_ID2          0x3E // 16 bit
+#define SN_ID3          0x3F // 16 bit
+
+#define T_STALL_US   20U
+
+#define SPI48_DATA_INT32(a)     (((int32_t)(((a) << 4)  & 0xfffff000UL)) >> 12)
+#define SPI48_DATA_UINT32(a)    ((uint32_t)(((a) >> 8)  & 0x000fffffUL))
+#define SPI48_DATA_UINT16(a)    ((uint16_t)(((a) >> 8)  & 0x0000ffffUL))
+
+extern const AP_HAL::HAL& hal;
+
+AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
+                                                         AP_HAL::OwnPtr<AP_HAL::Device> _dev,
+                                                         enum Rotation _rotation)
+    : AP_InertialSensor_Backend(imu)
+    , dev(std::move(_dev))
+    , rotation(_rotation)
+{
+    expected_sample_rate_hz = 1475;
+    accel_scale = 1.f / 1600.f;
+    gyro_scale = radians(1.f / 1600.f);
+
+    _registers[0] = RegisterConfig(CTRL_FILT_RATE,  FILTER_BYPASS);
+    _registers[1] = RegisterConfig(CTRL_FILT_ACC12, FILTER_BYPASS);
+    _registers[2] = RegisterConfig(CTRL_FILT_ACC3,  FILTER_BYPASS);
+    _registers[3] = RegisterConfig(CTRL_RATE,       RATE_300DPS_1475HZ); // +/- 300 deg/s, 1600 LSB/(deg/s) -- default, Decimation 8, 1475Hz
+    _registers[4] = RegisterConfig(CTRL_ACC12,      ACC12_8G_1475HZ);    // +/- 80 m/s^2, 3200 LSB/(m/s^2) -- default, Decimation 8, 1475Hz
+    _registers[5] = RegisterConfig(CTRL_ACC3,       ACC3_26G);           // +/- 260 m/s^2, 1600 LSB/(m/s^2) -- default
+}
+
+AP_InertialSensor_Backend *
+AP_InertialSensor_SCH16T::probe(AP_InertialSensor &imu,
+                                   AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                   enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+
+    auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation);
+
+    if (!sensor) {
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+void AP_InertialSensor_SCH16T::start()
+{
+    if (!_imu.register_accel(accel_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_SCH16T)) ||
+        !_imu.register_gyro(gyro_instance, expected_sample_rate_hz,   dev->get_bus_id_devtype(DEVTYPE_INS_SCH16T))) {
+        return;
+    }
+
+    // setup sensor rotations from probe()
+    set_gyro_orientation(gyro_instance, rotation);
+    set_accel_orientation(accel_instance, rotation);
+
+    uint32_t period_us = 1000000UL / expected_sample_rate_hz;
+    periodic_handle = dev->register_periodic_callback(period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SCH16T::run_state_machine, void));
+}
+
+void AP_InertialSensor_SCH16T::run_state_machine()
+{
+    WITH_SEMAPHORE(dev->get_semaphore());
+
+    switch (_state) {
+    case State::PowerOn: {
+            _state = State::Reset;
+            dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME);
+            break;
+        }
+
+    case State::Reset: {
+            failure_count = 0;
+            reset_chip();
+            _state = State::Configure;
+            dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME);
+            break;
+        }
+
+    case State::Configure: {
+            if (!read_product_id()) {
+                _state = State::Reset;
+                dev->adjust_periodic_callback(periodic_handle, 2000000); // 2s
+                break;
+            }
+
+            configure_registers();
+            _state = State::LockConfiguration;
+            dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME);
+            break;
+        }
+
+    case State::LockConfiguration: {
+            read_status_registers(); // Read all status registers once
+            register_write(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR
+            _state = State::Validate;
+            dev->adjust_periodic_callback(periodic_handle, 50000UL); // 50ms
+            break;
+        }
+
+    case State::Validate: {
+            read_status_registers(); // Read all status registers twice
+            read_status_registers();
+
+            // Check that registers are configured properly and that the sensor status is OK
+            if (validate_sensor_status() && validate_register_configuration()) {
+                _state = State::Read;
+                dev->adjust_periodic_callback(periodic_handle, 1000000UL / expected_sample_rate_hz);
+
+            } else {
+                _state = State::Reset;
+                dev->adjust_periodic_callback(periodic_handle, POWER_ON_TIME);
+            }
+
+            break;
+        }
+
+    case State::Read: {
+            if (collect_and_publish()) {
+                if (failure_count > 0) {
+                    failure_count--;
+                }
+            } else {
+                failure_count++;
+            }
+
+            // Reset if successive failures
+            if (failure_count > 10) {
+                _state = State::Reset;
+                return;
+            }
+
+            break;
+        }
+
+    default:
+        break;
+    } // end switch/case
+}
+
+bool AP_InertialSensor_SCH16T::collect_and_publish()
+{
+    SensorData data = {};
+    bool success = read_data(&data);
+    if (success) {
+        Vector3f accel{accel_scale*data.acc_x, accel_scale*data.acc_y, accel_scale*data.acc_z};
+        Vector3f gyro{gyro_scale*data.gyro_x, gyro_scale*data.gyro_y, gyro_scale*data.gyro_z};
+
+        _rotate_and_correct_accel(accel_instance, accel);
+        _notify_new_accel_raw_sample(accel_instance, accel);
+
+        _rotate_and_correct_gyro(gyro_instance, gyro);
+        _notify_new_gyro_raw_sample(gyro_instance, gyro);
+
+        _publish_temperature(accel_instance, float(data.temp)/100.f);
+
+        // adjust the periodic callback to be synchronous with the incoming data
+        dev->adjust_periodic_callback(periodic_handle, 1000000UL / expected_sample_rate_hz);
+    }
+
+    return success;
+}
+
+void AP_InertialSensor_SCH16T::reset_chip()
+{
+#if defined(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1)
+    palClearLine(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1);
+    hal.scheduler->delay(2000);
+    palSetLine(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1);
+#else
+    register_write(CTRL_RESET, SPI_SOFT_RESET);
+#endif
+}
+
+bool AP_InertialSensor_SCH16T::read_data(SensorData *data)
+{
+    register_read(RATE_X2);
+    uint64_t gyro_x = register_read(RATE_Y2);
+    uint64_t gyro_y = register_read(RATE_Z2);
+    uint64_t gyro_z = register_read(ACC_X3);
+    uint64_t acc_x  = register_read(ACC_Y3);
+    uint64_t acc_y  = register_read(ACC_Z3);
+    uint64_t acc_z  = register_read(TEMP);
+    uint64_t temp   = register_read(TEMP);
+
+    static constexpr uint64_t MASK48_ERROR = 0x001E00000000UL;
+    uint64_t values[] = { gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temp };
+
+    for (auto v : values) {
+        // Check for frame errors
+        if (v & MASK48_ERROR) {
+            return false;
+        }
+
+        // Validate the CRC
+        if (uint8_t(v & 0xff) != calculate_crc8(v)) {
+            return false;
+        }
+    }
+
+    // Data registers are 20bit 2s complement
+    data->acc_x    = SPI48_DATA_INT32(acc_x);
+    data->acc_y    = SPI48_DATA_INT32(acc_y);
+    data->acc_z    = SPI48_DATA_INT32(acc_z);
+    data->gyro_x   = SPI48_DATA_INT32(gyro_x);
+    data->gyro_y   = SPI48_DATA_INT32(gyro_y);
+    data->gyro_z   = SPI48_DATA_INT32(gyro_z);
+    // Temperature data is always 16 bits wide. Drop 4 LSBs as they are not used.
+    data->temp    = SPI48_DATA_INT32(temp) >> 4;
+
+    // Convert to RH coordinate system (FLU to FRD)
+    data->acc_x = data->acc_x;
+    data->acc_y = -data->acc_y;
+    data->acc_z = -data->acc_z;
+    data->gyro_x = data->gyro_x;
+    data->gyro_y = -data->gyro_y;
+    data->gyro_z = -data->gyro_z;
+
+    return true;
+}
+
+bool AP_InertialSensor_SCH16T::read_product_id()
+{
+    register_read(COMP_ID);
+    uint16_t comp_id = SPI48_DATA_UINT16(register_read(ASIC_ID));
+    uint16_t asic_id = SPI48_DATA_UINT16(register_read(ASIC_ID));
+
+    // Debug code
+    // register_read(SN_ID1);
+    // uint16_t sn_id1 = SPI48_DATA_UINT16(register_read(SN_ID2));
+    // uint16_t sn_id2 = SPI48_DATA_UINT16(register_read(SN_ID3));
+    // uint16_t sn_id3 = SPI48_DATA_UINT16(register_read(SN_ID3));
+
+    // char serial_str[14];
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3);
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Serial:\t %s", serial_str);
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "COMP_ID:\t 0x%0x", comp_id);
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ASIC_ID:\t 0x%0x", asic_id);
+
+    // SCH16T-K01   -   ID hex = 0x0020
+    // SCH1633-B13  -   ID hex = 0x0017
+    bool success = asic_id == 0x20 && comp_id == 0x17;
+
+    return success;
+}
+
+void AP_InertialSensor_SCH16T::configure_registers()
+{
+    for (auto &r : _registers) {
+        register_write(r.addr, r.value);
+    }
+
+    register_write(CTRL_USER_IF, DRY_DRV_EN); // Enable data ready
+    register_write(CTRL_MODE, EN_SENSOR); // Enable the sensor
+}
+
+bool AP_InertialSensor_SCH16T::validate_sensor_status()
+{
+    auto &s = _sensor_status;
+    uint16_t values[] = { s.summary, s.saturation, s.common, s.rate_common, s.rate_x, s.rate_y, s.rate_z, s.acc_x, s.acc_y, s.acc_z };
+
+    for (auto v : values) {
+        if (v != 0xFFFF) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool AP_InertialSensor_SCH16T::validate_register_configuration()
+{
+    bool success = true;
+
+    for (auto &r : _registers) {
+        register_read(r.addr); // double read, wasteful but makes the code cleaner, not high rate so doesn't matter anyway
+        auto value = SPI48_DATA_UINT16(register_read(r.addr));
+
+        if (value != r.value) {
+            success = false;
+        }
+    }
+
+    return success;
+}
+
+void AP_InertialSensor_SCH16T::read_status_registers()
+{
+    register_read(STAT_SUM);
+    _sensor_status.summary      = SPI48_DATA_UINT16(register_read(STAT_SUM_SAT));
+    _sensor_status.saturation   = SPI48_DATA_UINT16(register_read(STAT_COM));
+    _sensor_status.common       = SPI48_DATA_UINT16(register_read(STAT_RATE_COM));
+    _sensor_status.rate_common  = SPI48_DATA_UINT16(register_read(STAT_RATE_X));
+    _sensor_status.rate_x       = SPI48_DATA_UINT16(register_read(STAT_RATE_Y));
+    _sensor_status.rate_y       = SPI48_DATA_UINT16(register_read(STAT_RATE_Z));
+    _sensor_status.rate_z       = SPI48_DATA_UINT16(register_read(STAT_ACC_X));
+    _sensor_status.acc_x        = SPI48_DATA_UINT16(register_read(STAT_ACC_Y));
+    _sensor_status.acc_y        = SPI48_DATA_UINT16(register_read(STAT_ACC_Z));
+    _sensor_status.acc_z        = SPI48_DATA_UINT16(register_read(STAT_ACC_Z));
+}
+
+uint64_t AP_InertialSensor_SCH16T::register_read(uint8_t addr)
+{
+    uint64_t frame = {};
+    frame |= uint64_t(addr) << 38; // Target address offset
+    frame |= uint64_t(1) << 35; // FrameType: SPI48BF
+    frame |= uint64_t(calculate_crc8(frame));
+
+    return transfer_spi_frame(frame);
+}
+
+// Non-data registers are the only writable ones and are 16 bit or less
+void AP_InertialSensor_SCH16T::register_write(uint8_t addr, uint16_t value)
+{
+    uint64_t frame = {};
+    frame |= uint64_t(1) << 37; // Write bit
+    frame |= uint64_t(addr) << 38; // Target address offset
+    frame |= uint64_t(1) << 35; // FrameType: SPI48BF
+    frame |= uint64_t(value) << 8;
+    frame |= uint64_t(calculate_crc8(frame));
+
+    // We don't care about the return frame on a write
+    (void)transfer_spi_frame(frame);
+}
+
+// The SPI protocol (SafeSPI) is 48bit out-of-frame. This means read return frames will be received on the next transfer.
+uint64_t AP_InertialSensor_SCH16T::transfer_spi_frame(uint64_t frame)
+{
+    uint16_t buf[3];
+    for (int index = 0; index < 3; index++) {
+        uint16_t lower_byte = (frame >> (index << 4)) & 0xFF;
+        uint16_t upper_byte = (frame >> ((index << 4) + 8)) & 0xFF;
+        buf[3 - index - 1] = (lower_byte << 8) | upper_byte;
+    }
+
+    dev->transfer((uint8_t*)buf, 6, (uint8_t*)buf, 6);
+
+    uint64_t value = {};
+    for (int index = 0; index < 3; index++) {
+        uint16_t lower_byte = buf[index] & 0xFF;
+        uint16_t upper_byte = (buf[index] >> 8) & 0xFF;
+        value |= (uint64_t)(upper_byte | (lower_byte << 8)) << ((3 - index - 1) << 4);
+    }
+
+    return value;
+}
+
+uint8_t AP_InertialSensor_SCH16T::calculate_crc8(uint64_t frame)
+{
+    uint64_t data = frame & 0xFFFFFFFFFF00LL;
+    uint8_t crc = 0xFF;
+
+    for (int i = 47; i >= 0; i--) {
+        uint8_t data_bit = data >> i & 0x01;
+        crc = crc & 0x80 ? (uint8_t)((crc << 1) ^ 0x2F) ^ data_bit : (uint8_t)(crc << 1) | data_bit;
+    }
+
+    return crc;
+}
+
+bool AP_InertialSensor_SCH16T::update()
+{
+    update_accel(accel_instance);
+    update_gyro(gyro_instance);
+    return true;
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
new file mode 100644
index 00000000000000..5dbff35955f2bb
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
@@ -0,0 +1,116 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
+public:
+    static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
+                                            AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                            enum Rotation rotation);
+
+    /**
+     * Configure the sensors and start reading routine.
+     */
+    void start() override;
+    bool update() override;
+
+private:
+
+    AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
+                                AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                enum Rotation rotation);
+
+    struct SensorData {
+        int32_t acc_x;
+        int32_t acc_y;
+        int32_t acc_z;
+        int32_t gyro_x;
+        int32_t gyro_y;
+        int32_t gyro_z;
+        int32_t temp;
+    };
+
+    struct SensorStatus {
+        uint16_t summary;
+        uint16_t saturation;
+        uint16_t common;
+        uint16_t rate_common;
+        uint16_t rate_x;
+        uint16_t rate_y;
+        uint16_t rate_z;
+        uint16_t acc_x;
+        uint16_t acc_y;
+        uint16_t acc_z;
+    };
+
+    struct RegisterConfig {
+        RegisterConfig() = default;
+        RegisterConfig(uint16_t a, uint16_t v)
+            : addr(a)
+            , value(v)
+        {};
+        uint8_t addr;
+        uint16_t value;
+    };
+
+    void run_state_machine(void);
+    bool collect_and_publish();
+
+    void reset_chip();
+    bool read_product_id();
+    void read_status_registers();
+    bool validate_sensor_status();
+    void configure_registers();
+    bool validate_register_configuration();
+
+    bool read_data(SensorData *data);
+
+    void register_write(uint8_t addr, uint16_t value);
+    uint64_t register_read(uint8_t addr);
+    uint64_t transfer_spi_frame(uint64_t frame);
+    uint8_t calculate_crc8(uint64_t frame);
+
+    enum class State : uint8_t {
+        PowerOn,
+        Reset,
+        Configure,
+        LockConfiguration,
+        Validate,
+        Read,
+    } _state = State::PowerOn;
+
+    RegisterConfig _registers[6];
+    SensorStatus _sensor_status;
+
+    AP_HAL::OwnPtr<AP_HAL::Device> dev;
+    AP_HAL::Device::PeriodicHandle periodic_handle;
+
+    uint8_t accel_instance {};
+    uint8_t gyro_instance = {};
+    enum Rotation rotation {};
+    uint8_t drdy_pin {};
+
+    int failure_count {};
+    float expected_sample_rate_hz {};
+
+    float accel_scale {};
+    float gyro_scale {};
+};

From 0dd430753f7b1d36a2355d43b0fdb275940b0967 Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Wed, 11 Mar 2026 17:00:09 +0200
Subject: [PATCH 14/19] wip

---
 .../hwdef/MicoAir743-Lite/hwdef-bl.dat        |   9 +-
 .../hwdef/MicoAir743-Lite/hwdef.dat           |  73 ++-
 .../AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat  |  42 +-
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c   |  31 +-
 .../AP_InertialSensor_SCH16T.cpp              | 572 ++++++++++++++++--
 .../AP_InertialSensor_SCH16T.h                |  50 +-
 6 files changed, 694 insertions(+), 83 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef-bl.dat
index 9df29b412a59ac..a9091a21edc633 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef-bl.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef-bl.dat
@@ -40,4 +40,11 @@ PD4 ICM45686_CS       CS
 # LEDs
 PE2 LED_ACTIVITY   OUTPUT LOW #green
 PE4 LED_BOOTLOADER OUTPUT LOW #blue
-define HAL_LED_ON 0
\ No newline at end of file
+define HAL_LED_ON 0
+
+
+undef PE2
+PE3 LED_ACTIVITY   OUTPUT LOW #red
+PA7  SCH16T_CS    CS
+
+PB1 SCH16T_RESET  OUTPUT HIGH # M5
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
index f19a3c5ce2d7b0..726339acc0f407 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
@@ -120,17 +120,17 @@ PC11 SDMMC1_D3  SDMMC1
 define FATFS_HAL_DEVICE SDCD1
 
 # SPI3 - ICM45686
-PB3  SPI3_SCK      SPI3
-PB4  SPI3_MISO     SPI3
-PD6  SPI3_MOSI     SPI3
-PD4  ICM45686_CS   CS
-PD5  ICM45686_DRDY INPUT
+#PB3  SPI3_SCK      SPI3
+#PB4  SPI3_MISO     SPI3
+#PD6  SPI3_MOSI     SPI3
+#PD4  ICM45686_CS   CS
+#PD5  ICM45686_DRDY INPUT
 
 # SPI devices
-SPIDEV  icm45686    SPI3  DEVID1  ICM45686_CS    MODE3   2*MHZ 8*MHZ
+#SPIDEV  icm45686    SPI3  DEVID1  ICM45686_CS    MODE3   2*MHZ 8*MHZ
 
 # 2 IMUs
-IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
+#IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
 
 # barometers
 BARO SPL06 I2C:0:0x77
@@ -162,3 +162,62 @@ undef PD14
 # fixme - consider PULLDOWN
 PD14  GPS_PPS INPUT FLOATING GPIO(64) #M15
 define HAL_GPIO_PPS 64
+
+
+# SCH16T and fake external SPI
+undef PE2
+undef PE5
+undef PE6
+#not undef PB14
+#not undef PB15
+undef PA7
+undef PA6
+undef PB1 # M5 used as SCH16 reset pin
+
+# SPI4 - External IMU (green LED channel approach)
+# Wiring:
+#   SCK  = PE2  (tap the MCU-side of the GREEN LED series resistor after removing it)
+#PA7  TIM3_CH2  TIM3  PWM(7)  GPIO(56) BIDIR
+#PA6  TIM3_CH1  TIM3  PWM(8)  GPIO(57)
+#   CS   = PA7  (M7 signal pin)
+#   DRDY = PA6  (M8 signal pin)
+#   MISO = PE5  (M9 signal pin)
+#   MOSI = PE6  (M10 signal pin)
+PE2  SPI4_SCK      SPI4
+PE5  SPI4_MISO     SPI4
+PE6  SPI4_MOSI     SPI4
+PA7  SCH16T_CS    CS
+PA6 SCH16T_DRDY  INPUT GPIO(57) # M8
+define SCH16T_DRDY_PIN 57
+
+PB1 SCH16T_RESET  OUTPUT HIGH GPIO(54) # M5
+define SCH16T_RESET_PIN 54
+
+
+# 1*MHZ 2*MHZ is too slow
+# 10MHZ 10Mh` is failing ?
+# FIXME swicth to high speed mode later on? was 8 8
+SPIDEV  extsch16t     SPI4  DEVID1  SCH16T_CS     MODE0   2*MHZ 8*MHZ
+
+# e.g.:
+#  IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
+# IMU <driver> SPI:ext_imu ROTATION_ROLL_180_YAW_90
+
+# FIXME no DRDY in current driver, compare driver with https://github.com/ArduPilot/ardupilot/commit/62727a05136948495325475be6a98ee3dbf1a085#diff-dc1e749347e6cf45fd4a92c928f301de27775c72e8e177a707315d099d2324a0
+#define EXT_IMU_DRDY_PIN ((uint8_t)62)
+#IMU SCH16T      SPI:extsch16t ROTATION_NONE EXT_IMU_DRDY_PIN
+
+IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN
+
+#define AP_CLOCK_CORRECTION_PPM 1700
+
+# hack the frequency to match PPS?
+#undef CH_CFG_ST_FREQUENCY
+#define CH_CFG_ST_FREQUENCY 1001700
+#undef CH_CFG_ST_RESOLUTION
+#define CH_CFG_ST_RESOLUTION 32
+
+
+//undef OSCILLATOR_HZ
+//OSCILLATOR_HZ 8013600
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
index 396599565520f7..9c096fe6c0773e 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
@@ -129,13 +129,13 @@ PH5 SP2_CS1 CS
 PA10 SP2_DRDY2 INPUT
 
 # SPI3 - BMI088
-PB2  SPI3_MOSI SPI3
-PC10 SPI3_SCK  SPI3
-PC11 SPI3_MISO SPI3
-PI4 SP3_CS1 CS
-PI8 SP3_CS2 CS
-PI6 SP3_DRDY1 INPUT
-PI7 SP3_DRDY2 INPUT GPIO(93)
+#PB2  SPI3_MOSI SPI3
+#PC10 SPI3_SCK  SPI3
+#PC11 SPI3_MISO SPI3
+#PI4 SP3_CS1 CS
+#PI8 SP3_CS2 CS
+#PI6 SP3_DRDY1 INPUT
+#PI7 SP3_DRDY2 INPUT GPIO(93)
 define SP3_DRDY2 93
 
 # SPI4 - unused
@@ -152,10 +152,10 @@ PF11 SPI5_MOSI SPI5
 PG7  FRAM_CS CS
 
 # SPI6 - external1 (disabled to save DMA channels)
-# PB3  SPI6_SCK SPI6
-# PA6  SPI6_MISO SPI6
-# PG14 SPI6_MOSI SPI6
-# PI10   EXT1_CS CS
+PB3  SPI6_SCK SPI6
+PA6  SPI6_MISO SPI6
+PG14 SPI6_MOSI SPI6
+PI10   EXT1_CS CS
 
 # PWM output pins
 PI0  TIM5_CH4 TIM5 PWM(1) GPIO(50)
@@ -307,10 +307,10 @@ define HAL_HEATER_MAG_OFFSET_BMM150 AP_HAL::Device::make_bus_id(AP_HAL::Device::
 define HAL_HEATER_MAG_OFFSET {HAL_HEATER_MAG_OFFSET_RM3100, HAL_HEATER_MAG_OFFSET_BMM150}
 
 # IMU devices for Holybro6X
-SPIDEV bmi088_g SPI3 DEVID1  SP3_CS2  MODE3 10*MHZ 10*MHZ
-SPIDEV bmi088_a SPI3 DEVID2  SP3_CS1  MODE3 10*MHZ 10*MHZ
+#SPIDEV bmi088_g SPI3 DEVID1  SP3_CS2  MODE3 10*MHZ 10*MHZ
+#SPIDEV bmi088_a SPI3 DEVID2  SP3_CS1  MODE3 10*MHZ 10*MHZ
 # alternative to bmi088
-SPIDEV icm20649  SPI3 DEVID1  SP3_CS1 MODE3  10*MHZ 10*MHZ
+#SPIDEV icm20649  SPI3 DEVID1  SP3_CS1 MODE3  10*MHZ 10*MHZ
 SPIDEV icm42688  SPI2 DEVID1  SP2_CS1 MODE3  2*MHZ  8*MHZ
 SPIDEV icm42670  SPI1 DEVID1  SP1_CS1 MODE3  2*MHZ  8*MHZ
 
@@ -323,15 +323,17 @@ SPIDEV icm20649_2 SPI1 DEVID1  SP1_CS1     MODE3  2*MHZ  8*MHZ
 SPIDEV ramtron   SPI5 DEVID1  FRAM_CS     MODE3  8*MHZ  8*MHZ
 
 # IMU devices for Holybro6X-Rev6
-SPIDEV adis16470 SPI3 DEVID1 SP3_CS1 MODE3 1*MHZ 2*MHZ
+#SPIDEV adis16470 SPI3 DEVID1 SP3_CS1 MODE3 1*MHZ 2*MHZ
 SPIDEV iim42652  SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ
 SPIDEV icm45686  SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ
 
 # IMU devices for Holybro6X-45686 version
-SPIDEV icm45686-3 SPI3 DEVID1 SP3_CS1 MODE3 2*MHZ 8*MHZ
+#SPIDEV icm45686-3 SPI3 DEVID1 SP3_CS1 MODE3 2*MHZ 8*MHZ
 SPIDEV icm45686-2 SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ
 SPIDEV icm45686-1 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ
 
+SPIDEV  SCH16T     SPI6  DEVID1  EXT1_CS     MODE0   1*MHZ 2*MHZ
+
 # Holybro6X 3 IMUs
 IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
 IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X)
@@ -350,9 +352,15 @@ IMU Invensensev3 SPI:icm45686  ROTATION_YAW_90             BOARD_MATCH(FMUV6_BOA
 
 # Holybro6X-45686 3 IMUs, the ones on SPI-2 and SPI-3 are isolated
 IMU Invensensev3 SPI:icm45686-2  ROTATION_ROLL_180  BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686)
-IMU Invensensev3 SPI:icm45686-3  ROTATION_ROLL_180  BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686)
+#IMU Invensensev3 SPI:icm45686-3  ROTATION_ROLL_180  BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686)
 IMU Invensensev3 SPI:icm45686-1  ROTATION_YAW_90    BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686)
 
+# FIXME implement DRDY
+define ADIS_DRDY_PIN 93
+#PD11 DRDY_ADIS16507 INPUT GPIO(93)
+IMU SCH16T      SPI:SCH16T ROTATION_NONE #ADIS_DRDY_PIN
+
+
 define HAL_INS_HIGHRES_SAMPLE 7
 define HAL_DEFAULT_INS_FAST_SAMPLE 7
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
index f1e510ccebd07e..9fdd3af0a6cf13 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -27,6 +27,33 @@
 
 #include "../../../AP_Math/div1000.h"
 
+#ifndef AP_CLOCK_CORRECTION_PPM
+#define AP_CLOCK_CORRECTION_PPM 0
+#endif
+
+static inline uint64_t clock_correct_us(uint64_t us)
+{
+#if AP_CLOCK_CORRECTION_PPM > 0
+    // clock is fast: return a slightly smaller time
+    return us - (us * (uint64_t)AP_CLOCK_CORRECTION_PPM) / 1000000ULL;
+#elif AP_CLOCK_CORRECTION_PPM < 0
+    // clock is slow: return a slightly larger time
+    return us + (us * (uint64_t)(-AP_CLOCK_CORRECTION_PPM)) / 1000000ULL;
+#else
+    return us;
+#endif
+}
+
+static inline uint32_t clock_correct_us32(uint32_t us)
+{
+#if AP_CLOCK_CORRECTION_PPM != 0
+    return (uint32_t)clock_correct_us((uint64_t)us);
+#else
+    return us;
+#endif
+}
+
+
 /*
   we have 4 possible configurations of boards, made up of boards that
   have the following properties:
@@ -64,7 +91,7 @@ static uint32_t system_time_u32_us(void)
 #if CH_CFG_ST_FREQUENCY != 1000000U
     now *= 1000000U/CH_CFG_ST_FREQUENCY;
 #endif
-    return now;
+    return clock_correct_us32(now);
 }
 #else
 #error "unsupported timer resolution"
@@ -93,7 +120,7 @@ static uint64_t hrt_micros64I(void)
 #ifdef AP_BOARD_START_TIME
     ret += AP_BOARD_START_TIME;
 #endif
-    return ret;
+    return clock_correct_us(ret);
 }
 
 static inline bool is_locked(void) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
index a04956571d3fd1..056ee176562d80 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -16,20 +16,87 @@
 #include <AP_Math/AP_Math.h>
 
 #include "AP_InertialSensor_SCH16T.h"
-// #include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS.h>
 
-#if defined(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1)
+
+//#if defined(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1)
 #include <hal.h>
-#endif
+//#endif
 
 static constexpr uint16_t EOI = (1 << 1);               // End of Initialization
 static constexpr uint16_t EN_SENSOR = (1 << 0);         // Enable RATE and ACC measurement
 static constexpr uint16_t DRY_DRV_EN = (1 << 5);        // Enables Data ready function
+
 static constexpr uint16_t FILTER_68HZ = (0x0000);       // 68 Hz default filter
 static constexpr uint16_t FILTER_BYPASS = (0b0000000111111111);     // No filtering
-static constexpr uint16_t RATE_300DPS_1475HZ = 0b0001001011011011; // Gyro XYZ range 300 deg/s @ 1475Hz
-static constexpr uint16_t ACC12_8G_1475HZ = 0b0001001011011011;  // Acc XYZ range 8 G and 1475 update rate
-static constexpr uint16_t ACC3_26G = (0b000 << 0);
+
+
+// page 46 (53)
+// CTRL_RATE - DYN1 (default) '001'
+
+// page 34 (53): 48-bit mode operations
+// The SPI binary frame is constructed as follows:
+// (TA9 TA8 TA[7:0] RW 0 FrTyp AE[6:0] DATA[19:0] CRC8)
+// in doc DATA[19:0] =                      00000 010 010 dec=[000 000 000] for Set RATE Dynamic Range to ±315°/s
+
+
+// dyn_rate=[001 001] dec=[011 011 011]
+static constexpr uint16_t RATE_300DPS_1475HZ = 0b0001001011011011; // Gyro XYZ range 300 deg/s @ 1475Hz - Data 15-bit 
+// CTRL_ACC12 - Set ACC Dynamic Range to ±80m/s2
+// sample in doc =                          00000 010 010 000 000 000
+// Settings for ACC_X12, ACC_Y12, ACC_Z12, post-processing decimation ratio, and shift value (dynamic range)
+
+// dyn_rate=[001 001] decimation=[011, 011, 011]
+static constexpr uint16_t ACC12_8G_1475HZ    = 0b0001001011011011;  // Acc XYZ range 8 G and 1475 update rate - Data 15-bit 
+// Measurement Range (m/s2 ) = ±80 ; ±260 100 Nominal Sensitivity, 20-bit (LSB/(m/s2)) = 1600
+
+// from PX4: 
+// static constexpr uint8_t DECIMATION_1475_HZ =	(0b011);
+// static constexpr uint8_t DECIMATION_738_HZ =	(0b100);
+
+// union RATE_CTRL_Register {
+// 	struct {
+// 		uint16_t DEC_RATE_X2  : 3;
+// 		uint16_t DEC_RATE_Y2  : 3;
+// 		uint16_t DEC_RATE_Z2  : 3;
+//
+// 		uint16_t DYN_RATE_XYZ2: 3;
+// 		uint16_t DYN_RATE_XYZ1: 3;
+// 		uint16_t reserved     : 1;
+// 	} bits;
+
+// 	uint16_t value;
+// };
+
+// union ACC12_CTRL_Register {
+// 	struct {
+// 		uint16_t DEC_ACC_X2  : 3;
+// 		uint16_t DEC_ACC_Y2  : 3;
+// 		uint16_t DEC_ACC_Z2  : 3;
+//
+// 		uint16_t DYN_ACC_XYZ2: 3;
+// 		uint16_t DYN_ACC_XYZ1: 3;
+// 		uint16_t reserved     : 1;
+// 	} bits;
+
+// 	uint16_t value;
+// };
+
+// 	// We always use the maximum dynamic range for gyro and accel
+// // Dynamic range settings
+// static constexpr uint8_t RATE_RANGE_300 = (0b001);
+// static constexpr uint8_t ACC12_RANGE_80 = (0b001);
+// static constexpr uint8_t ACC3_RANGE_260 = (0b000);
+
+	// rate_ctrl.bits.DYN_RATE_XYZ1 = 	RATE_RANGE_300;
+	// rate_ctrl.bits.DYN_RATE_XYZ2 = 	RATE_RANGE_300;
+
+	// acc12_ctrl.bits.DYN_ACC_XYZ1 = 	ACC12_RANGE_80;
+	// acc12_ctrl.bits.DYN_ACC_XYZ2 = 	ACC12_RANGE_80;
+	// acc3_ctrl.bits.DYN_ACC_XYZ3 = 	ACC3_RANGE_260;
+
+
+static constexpr uint16_t ACC3_26G = (0b000 << 0); // Data is 3 bit, default. 
 static constexpr uint16_t SPI_SOFT_RESET = (0b1010);
 static constexpr uint32_t POWER_ON_TIME = 250000UL;
 
@@ -89,33 +156,74 @@ extern const AP_HAL::HAL& hal;
 
 AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
                                                          AP_HAL::OwnPtr<AP_HAL::Device> _dev,
-                                                         enum Rotation _rotation)
+                                                         enum Rotation _rotation,
+                                                         uint8_t _drdy_pin,
+                                                        uint8_t _reset_pin)
     : AP_InertialSensor_Backend(imu)
     , dev(std::move(_dev))
     , rotation(_rotation)
+    , drdy_pin(_drdy_pin)
+    , reset_pin(_reset_pin)
 {
-    expected_sample_rate_hz = 1475;
-    accel_scale = 1.f / 1600.f;
+    // on K01:  according to datasheet,
+    // the update rate is 23.6/X (1 kHz max) where X is the decimation factor set in the control registers. 
+    // "1) Decimation ratio X is selectable from the following options: 2, 4, 8, 16 and 32"
+    // With the default settings we have decimation of 16, which gives us an update rate of ~1475Hz.
+    // Table 15 Selectable decimation ratios and corresponding ODR
+    // Decimation factor        Output data rate        Output data rate with nominal F_PRIM (kHz)
+    // 1                        F_PRIM/2                11.8
+    // 2                        F_PRIM/4                 5.9
+    // 4                        F_PRIM/8                 2.95
+    // 8                        F_PRIM/16                1.475
+    // 16                       F_PRIM/32                0.7375
+
+    // Drawback of decimation is that sampling jitter is increased with the same ratio as the decimation factor.
+    // With nominal primary frequency and decimation ratio of 16, the sampling jitter will be up to 85 µs x 16 =
+    // 1.36 ms. This means that sample age can be anything between 0 and 1.36 ms. To address this issue,
+    // the user can combine decimation with the data ready function. Data ready is explained in chapter 5.4.4.
+
+    // However currently we get ~737-738Hz
+    expected_sample_rate_hz = 1475; //FIXME?
+
+    // if using ACC3 with 26G range, the sensitivity is 1600 LSB/(m/s^2), so accel_scale = 1.f / 1600.f;
+    // accel_scale = 1.f / 1600.f;
+
+    // if using ACC2 with 8G range, the sensitivity is 3200 LSB/(m/s^2), so accel_scale = 1.f / 3200.f;
+    accel_scale = 1.f / 3200.f;
+
     gyro_scale = radians(1.f / 1600.f);
 
     _registers[0] = RegisterConfig(CTRL_FILT_RATE,  FILTER_BYPASS);
     _registers[1] = RegisterConfig(CTRL_FILT_ACC12, FILTER_BYPASS);
     _registers[2] = RegisterConfig(CTRL_FILT_ACC3,  FILTER_BYPASS);
+    // CTRL_RATE Settings for Gyro post-processing, decimation ratio, and dynamic range RW    15h0028, D0
     _registers[3] = RegisterConfig(CTRL_RATE,       RATE_300DPS_1475HZ); // +/- 300 deg/s, 1600 LSB/(deg/s) -- default, Decimation 8, 1475Hz
+    // Settings for ACC_X12, ACC_Y12, ACC_Z12 post-processing decimation ratio, and shift value (dynamic range)
+    // ACC12 dynamic range settings
     _registers[4] = RegisterConfig(CTRL_ACC12,      ACC12_8G_1475HZ);    // +/- 80 m/s^2, 3200 LSB/(m/s^2) -- default, Decimation 8, 1475Hz
+    // Settings for ACC_X3, ACC_Y3, ACC_Z3 post-processing shift value (dynamic range)
+    // ACC3 dynamic range settings
+    // K01:
+    //  Measurement Range (m/s2 ) = ±80 ; 
+    // Electrical headroom = ±260 
+    // Nominal Sensitivity, 16-bit (LSB/(m/s2)) = 100 
+    // Nominal Sensitivity, 20-bit (LSB/(m/s2)) = 1600
     _registers[5] = RegisterConfig(CTRL_ACC3,       ACC3_26G);           // +/- 260 m/s^2, 1600 LSB/(m/s^2) -- default
 }
 
 AP_InertialSensor_Backend *
 AP_InertialSensor_SCH16T::probe(AP_InertialSensor &imu,
                                    AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                   enum Rotation rotation)
+                                   enum Rotation rotation,
+                                   uint8_t drdy_gpio,
+                                   uint8_t reset_gpio
+                                )
 {
     if (!dev) {
         return nullptr;
     }
 
-    auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation);
+    auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation, drdy_gpio, reset_gpio);
 
     if (!sensor) {
         return nullptr;
@@ -136,13 +244,247 @@ void AP_InertialSensor_SCH16T::start()
     set_accel_orientation(accel_instance, rotation);
 
     uint32_t period_us = 1000000UL / expected_sample_rate_hz;
-    periodic_handle = dev->register_periodic_callback(period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SCH16T::run_state_machine, void));
+
+    bool use_thread = true;
+
+    if (!use_thread) {
+        periodic_handle = dev->register_periodic_callback(period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SCH16T::run_state_machine, void));
+    } else {
+        /*
+          as the sensor does not have a FIFO we need to jump through some
+          hoops to ensure we don't lose any samples. This creates a thread
+          to do the capture, running at very high priority
+         */
+        if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SCH16T::loop_thread, void),
+                                          "SCH16T",
+                                          1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
+            AP_HAL::panic("Failed to create SCH16T thread");
+        }
+    }
+
 }
 
-void AP_InertialSensor_SCH16T::run_state_machine()
+void sch16t_delay_us(uint32_t usec)
 {
+    if (usec < 65535UL) {
+        hal.scheduler->delay_microseconds(usec);
+        return;
+    } else {
+        // for longer delays, we need to split into multiple calls, as it takes a uint16_t
+        hal.scheduler->delay((uint16_t) (usec / (uint32_t)1000));
+        hal.scheduler->delay_microseconds((uint16_t) (usec % (uint32_t)1000));
+    }
+}
+
+
+void AP_InertialSensor_SCH16T::perform_read(){
+    // no semaphore here?
+    const uint32_t period_us = (1000000UL / expected_sample_rate_hz) - 20U;
+    ReadStatus read_status = {};
+    while(_state == State::Read) {
+        uint32_t tstart = AP_HAL::micros();
+        WITH_SEMAPHORE(dev->get_semaphore());
+        uint32_t event_time = tstart;
+        bool drdy_wait_ok = false;
+        bool wait_ok = true;
+        if (drdy_pin != 0) {
+            // when we have a DRDY pin then wait for it to go high
+            drdy_wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, period_us+50U);
+            if (drdy_wait_ok){
+                wait_ok = false;
+                num_drydry_ok++;
+            }
+            event_time = AP_HAL::micros();
+        }
+        // FIMXE: getting huge variation in accel readings...
+        if (collect_and_publish(true, event_time, &read_status)) {
+            if (failure_count > 0) {
+                failure_count--;
+            }
+            if (!wait_ok) {
+                uint32_t dt = AP_HAL::micros() - event_time;
+                // moving average
+                num_dt_items++;
+                total_dt += dt;
+                min_dt = min_dt > dt ? dt : min_dt;
+                max_dt = max_dt < dt ? dt : max_dt;
+            }
+            num_ok_samples++;
+            // log status every few seconds
+            if (num_ok_samples % (1500*100) == 0) {
+                if (true) { 
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, 
+                        "SCH16T: %llu, %llu DRDY, %llu miss, %llu mixed, %llu no new, %d fail, avg_time=%llu us (min=%lu max=%lu, max report=%lu read=%lu crc8=%lu spi=%lu over %llu)",//, avg_wait=%llu us (over %llu)",
+                        num_ok_samples, num_drydry_ok, num_missed_samples, num_inconsistent_samples, num_no_new_samples, failure_count, 
+                        num_dt_items > 0 ? total_dt / num_dt_items : 0, min_dt, max_dt, max_reporting_dt, max_read_data_dt, max_crc8_dt, max_read_spi_dt, num_dt_items
+                        //num_wait > 0 ? total_wait_time_us / num_wait : 0, num_wait
+                    );
+                }
+                num_ok_samples = 0;
+                num_missed_samples = 0;
+                num_inconsistent_samples = 0;
+                num_no_new_samples = 0;
+
+                num_dt_items = 0;
+                total_dt = 0;
+                min_dt = 100000;
+                max_dt = 0;
+                max_reporting_dt = 0;
+                max_read_data_dt = 0;
+                max_read_spi_dt = 0;
+                max_crc8_dt = 0;
+
+                num_wait = 0;
+                total_wait_time_us = 0;
+
+                num_drydry_ok = 0;
+            }
+        } else {
+            if (read_status.warn_no_new_sample) {
+                // we didn't get a new sample, but it's not an error, just wait for the next one
+                wait_ok = false;
+                num_no_new_samples++;
+                // read again almost immediately, expect that we'll get better alignment in time
+                // without delay here, sensor keeps restarting
+                if (drdy_pin == 0) {
+                    sch16t_delay_us(1);
+                }
+            } else if (read_status.err_sample_inconsistent) {
+                // we got a sample but it was inconsistent, so we should try to read again immediately
+                // without waiting, as we might have just caught the sample in the middle of being updated
+                // and the next read might be consistent
+                wait_ok = false;
+                failure_count++;
+                num_inconsistent_samples++;
+                if (drdy_pin == 0) {
+                    sch16t_delay_us(1);
+                }
+            } else {
+                // some other error, we should reset the sensor
+                failure_count++;
+            }
+        }
+
+        // Reset if successive failures
+        if (failure_count > 100) {
+            _state = State::Reset;
+            return;
+        } else {
+            if (wait_ok) {
+                uint32_t dt = AP_HAL::micros() - tstart;
+                // moving average
+                num_dt_items++;
+                total_dt += dt;
+                
+                // FIXME: this is strangely sensitive to -20U value
+                            
+                if (dt < period_us) {
+                    uint32_t wait_us = period_us - dt;
+                    if (!drdy_wait_ok || wait_us >= 1U) {
+                        num_wait++;
+                        total_wait_time_us += wait_us;
+                        sch16t_delay_us(wait_us);
+                    }
+                }
+            }
+        }
+    }
+}
+
+
+void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
+{
+
     WITH_SEMAPHORE(dev->get_semaphore());
 
+    // like run_state_machine, just uses delay instead of periodic callback, 
+    // to be used if the driver is configured to run in a thread instead of using periodic callbacks
+    switch (_state) {
+    case State::PowerOn: {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron");
+            _state = State::Reset;
+            sch16t_delay_us(POWER_ON_TIME);
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron delay done");
+            break;
+        }
+
+    case State::Reset: {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: resetting");
+            failure_count = 0;
+            reset_chip();
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset done");
+            _state = State::Configure;
+            sch16t_delay_us(POWER_ON_TIME);
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset delay done");
+            break;
+        }
+    
+    case State::Configure: {
+            if (!read_product_id()) {
+                _state = State::Reset;
+                sch16t_delay_us(2000000); // 2s
+                break;
+            }
+
+            configure_registers();
+            _state = State::LockConfiguration;
+            sch16t_delay_us(POWER_ON_TIME);
+            break;
+        }
+
+    case State::LockConfiguration: {
+            read_status_registers(); // Read all status registers once
+            register_write(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR
+            _state = State::Validate;
+            sch16t_delay_us(50000UL); // 50ms
+            break;
+        }
+
+    case State::Validate: {
+            read_status_registers(); // Read all status registers twice
+            read_status_registers();
+
+            // Check that registers are configured properly and that the sensor status is OK
+            if (validate_sensor_status() && validate_register_configuration()) {
+                _state = State::Read;
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: validated, will read");
+            } else {
+                _state = State::Reset;
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: failed to validate, resetting again");
+                sch16t_delay_us(POWER_ON_TIME);
+            }
+
+            break;
+        }
+
+    case State::Read: {
+            break;
+        }
+    
+    default:
+        break;
+    } // end switch/case
+}
+
+void AP_InertialSensor_SCH16T::loop_thread(void)
+{
+    while (true) {
+        if (_state == State::Read) {
+            perform_read();
+        } else {
+            run_state_machine_non_periodic();
+        }
+        //hal.scheduler->delay_microseconds(T_STALL_US);
+    }
+}
+
+
+void AP_InertialSensor_SCH16T::run_state_machine()
+{
+    ReadStatus read_status = {};
+    // FIXME: is it good practice to use semaphore all the time?
+    //WITH_SEMAPHORE(dev->get_semaphore());
+
     switch (_state) {
     case State::PowerOn: {
             _state = State::Reset;
@@ -197,7 +539,7 @@ void AP_InertialSensor_SCH16T::run_state_machine()
         }
 
     case State::Read: {
-            if (collect_and_publish()) {
+            if (collect_and_publish(false, AP_HAL::micros(), &read_status)) {
                 if (failure_count > 0) {
                     failure_count--;
                 }
@@ -219,24 +561,32 @@ void AP_InertialSensor_SCH16T::run_state_machine()
     } // end switch/case
 }
 
-bool AP_InertialSensor_SCH16T::collect_and_publish()
+bool AP_InertialSensor_SCH16T::collect_and_publish(bool use_thread, uint64_t sample_time_us, ReadStatus *read_status)
 {
     SensorData data = {};
-    bool success = read_data(&data);
+    bool success = read_data(&data, read_status);
+    
     if (success) {
+        uint32_t reporting_start = AP_HAL::micros();
+        // adjust the periodic callback to be synchronous with the incoming data
+        // call this immediately after a read
+        if (!use_thread) {
+            dev->adjust_periodic_callback(periodic_handle, 1000000UL / expected_sample_rate_hz);
+        }
+
         Vector3f accel{accel_scale*data.acc_x, accel_scale*data.acc_y, accel_scale*data.acc_z};
         Vector3f gyro{gyro_scale*data.gyro_x, gyro_scale*data.gyro_y, gyro_scale*data.gyro_z};
 
         _rotate_and_correct_accel(accel_instance, accel);
-        _notify_new_accel_raw_sample(accel_instance, accel);
+        _notify_new_accel_raw_sample(accel_instance, accel, sample_time_us);
 
         _rotate_and_correct_gyro(gyro_instance, gyro);
-        _notify_new_gyro_raw_sample(gyro_instance, gyro);
+        _notify_new_gyro_raw_sample(gyro_instance, gyro, sample_time_us);
 
         _publish_temperature(accel_instance, float(data.temp)/100.f);
-
-        // adjust the periodic callback to be synchronous with the incoming data
-        dev->adjust_periodic_callback(periodic_handle, 1000000UL / expected_sample_rate_hz);
+        uint32_t reporting_end = AP_HAL::micros();
+        uint32_t reporting_dt = reporting_end - reporting_start;
+        max_reporting_dt = max_reporting_dt < reporting_dt ? reporting_dt : max_reporting_dt;
     }
 
     return success;
@@ -244,41 +594,143 @@ bool AP_InertialSensor_SCH16T::collect_and_publish()
 
 void AP_InertialSensor_SCH16T::reset_chip()
 {
-#if defined(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1)
-    palClearLine(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1);
-    hal.scheduler->delay(2000);
-    palSetLine(HAL_GPIO_PIN_nSPI6_RESET_EXTERNAL1);
-#else
-    register_write(CTRL_RESET, SPI_SOFT_RESET);
-#endif
+    if (reset_pin != 0) {
+        palClearLine(reset_pin);
+        hal.scheduler->delay(2000);
+        palSetLine(reset_pin);
+    } else {
+        register_write(CTRL_RESET, SPI_SOFT_RESET);
+    }
 }
 
-bool AP_InertialSensor_SCH16T::read_data(SensorData *data)
+inline bool AP_InertialSensor_SCH16T::validate_frame_errors(uint64_t value){
+    // Check for frame errors
+    // FIXME: need special handling for some errors, like:
+    // - out of scale / saturation
+    // - still loading / not ready
+    static constexpr uint64_t MASK48_ERROR = (uint64_t)0x001E00000000UL;
+     if (value & MASK48_ERROR) {
+        return false;
+    }
+    // Validate the CRC
+    if (uint8_t(value & 0xff) != calculate_crc8(value)) {
+        return false;
+    }
+    return true;
+}
+
+
+inline bool AP_InertialSensor_SCH16T::validate_received_frame(uint64_t value, int &last_dcnt, ReadStatus *read_status)
+{
+    if (!validate_frame_errors(value)) {
+        return false;
+    }
+
+    // bits 29-32 of the 48-bit SPI frame are used for data counter
+    //static constexpr uint64_t MASK48_DCNT   = 0b00001111UL << 29;
+    int data_counter = (int)(((value >> 29) & 0x00000000000FUL));
+
+    // we should have the same data counter for all values, otherwise we got a mixure of two samples
+    if (last_dcnt != -1 && data_counter != last_dcnt) {
+        read_status->warn_no_new_sample = false;
+        read_status->err_sample_inconsistent = true;
+        return false;
+    }
+
+    // check that we have a new sample, otherwise we are reading the same sample again, which is not good
+    if (data_counter == earlier_dcnt) {
+        // we are reading the same sample again
+        read_status->warn_no_new_sample = true;
+        read_status->err_sample_inconsistent = false;
+        return false;
+    }
+
+    last_dcnt = data_counter;
+    return true;
+}
+
+
+bool AP_InertialSensor_SCH16T::read_data(SensorData *data, ReadStatus *read_status)
 {
+    uint32_t read_data_start = AP_HAL::micros();
+    
+    // clean status
+    read_status->warn_no_new_sample = false;
+    read_status->err_sample_inconsistent = false;
+
+    // Data counter is supported for decimated outputs RATE_XYZ2 and ACC_XYZ2. Value of data counter is
+    // increased by one when a new sample is available from corresponding RATE/ACC channel. It can be
+    // understood as an index for the data output values. Using the data counter, the user can monitor that
+    // every wanted sample has been acquired and that the same sample has not been read twice.
+
+    // When using 48-bit SPI protocol, 4-bit data counter value is included in MISO response frame. 
+
+
+
+    // 5.4.6 Frequency counter
+    // Using frequency counter, user can acquire accurate clock information from component internal MCLK
+    // via SPI. The value of frequency counter register is increased by one with every 16th rising edge of
+    // master clock. 
+
+    // 5.4.7 Calculating exact time stamp
+    // The data counter value can be combined with the frequency counter value to calculate the exact time
+    // stamp of a sample when the MCU clock of the host system is used as reference. This combination is
+    // recommended if integration operations are performed to sensor data and timing uncertainty or data jitter
+    // of the interpolated data do not fulfill system accuracy requirements.
+
+    // FIXME: this doesn't check if we got an old sample!
+    // FIXME: reads look weird here, or is it expected?
+
+    // DCNT A wrapping 4-bit sensor data counter.
+
+    // Check errors & Validate data counter early on, so we can read again later with minimum delay
+    int last_dcnt = -1;
     register_read(RATE_X2);
     uint64_t gyro_x = register_read(RATE_Y2);
+    //if (!validate_received_frame(gyro_x, last_dcnt, read_status)) return false;
     uint64_t gyro_y = register_read(RATE_Z2);
-    uint64_t gyro_z = register_read(ACC_X3);
-    uint64_t acc_x  = register_read(ACC_Y3);
-    uint64_t acc_y  = register_read(ACC_Z3);
+    //if (!validate_received_frame(gyro_y, last_dcnt, read_status)) return false;
+    uint64_t gyro_z = register_read(ACC_X2);
+    //if (!validate_received_frame(gyro_z, last_dcnt, read_status)) return false;
+    uint64_t acc_x  = register_read(ACC_Y2);
+    //if (!validate_received_frame(acc_x, last_dcnt, read_status)) return false;
+    uint64_t acc_y  = register_read(ACC_Z2);
+    //if (!validate_received_frame(acc_y, last_dcnt, read_status)) return false;
     uint64_t acc_z  = register_read(TEMP);
+    //if (!validate_received_frame(acc_z, last_dcnt, read_status)) return false;
     uint64_t temp   = register_read(TEMP);
 
-    static constexpr uint64_t MASK48_ERROR = 0x001E00000000UL;
-    uint64_t values[] = { gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temp };
+    uint32_t read_spi_end = AP_HAL::micros();
+    uint32_t read_spi_dt = read_spi_end - read_data_start;
+    max_read_spi_dt = max_read_spi_dt < read_spi_dt ? read_spi_dt : max_read_spi_dt;
 
-    for (auto v : values) {
-        // Check for frame errors
-        if (v & MASK48_ERROR) {
-            return false;
-        }
+    if (!validate_received_frame(gyro_x, last_dcnt, read_status)) return false;
+    if (!validate_received_frame(gyro_y, last_dcnt, read_status)) return false;
+    if (!validate_received_frame(gyro_z, last_dcnt, read_status)) return false;
+    if (!validate_received_frame(acc_x, last_dcnt, read_status)) return false;
+    if (!validate_received_frame(acc_y, last_dcnt, read_status)) return false;
+    if (!validate_received_frame(acc_z, last_dcnt, read_status)) return false;
 
-        // Validate the CRC
-        if (uint8_t(v & 0xff) != calculate_crc8(v)) {
-            return false;
+
+    if (last_dcnt != -1) {
+        if (earlier_dcnt != -1) {
+            // check for missed samples
+            if ((earlier_dcnt + 1) % 16 != last_dcnt % 16){
+                num_missed_samples++;
+            }
         }
+        // we got a valid data counter, save it for the next read
+        earlier_dcnt = last_dcnt;
     }
 
+    if (!validate_frame_errors(temp)) {
+        return false;
+    }
+
+    uint32_t crc8_end = AP_HAL::micros();
+    uint32_t crc8_dt = crc8_end - read_spi_end;
+    max_crc8_dt = max_crc8_dt < crc8_dt ? crc8_dt : max_crc8_dt;
+
     // Data registers are 20bit 2s complement
     data->acc_x    = SPI48_DATA_INT32(acc_x);
     data->acc_y    = SPI48_DATA_INT32(acc_y);
@@ -297,6 +749,10 @@ bool AP_InertialSensor_SCH16T::read_data(SensorData *data)
     data->gyro_y = -data->gyro_y;
     data->gyro_z = -data->gyro_z;
 
+    uint32_t read_data_end = AP_HAL::micros();
+    uint32_t read_data_dt = read_data_end - read_data_start;
+    max_read_data_dt = max_read_data_dt < read_data_dt ? read_data_dt : max_read_data_dt;
+
     return true;
 }
 
@@ -312,17 +768,22 @@ bool AP_InertialSensor_SCH16T::read_product_id()
     // uint16_t sn_id2 = SPI48_DATA_UINT16(register_read(SN_ID3));
     // uint16_t sn_id3 = SPI48_DATA_UINT16(register_read(SN_ID3));
 
-    // char serial_str[14];
-    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3);
-    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Serial:\t %s", serial_str);
-    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "COMP_ID:\t 0x%0x", comp_id);
-    // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ASIC_ID:\t 0x%0x", asic_id);
+    //char serial_str[14];
+    //GCS_SEND_TEXT(MAV_SEVERITY_INFO, serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3);
+    //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Serial:\t %s", serial_str);
+    //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "COMP_ID:\t 0x%0x ASIC_ID:\t 0x%0x", comp_id, asic_id);
 
     // SCH16T-K01   -   ID hex = 0x0020
     // SCH1633-B13  -   ID hex = 0x0017
-    bool success = asic_id == 0x20 && comp_id == 0x17;
+    //bool success = asic_id == 0x20 && comp_id == 0x17;
+    // asic_id - ASIC revision
+    // comp_id - Component type
+    bool ok = asic_id == 0x21 || comp_id == 0x23;
 
-    return success;
+    // FIXME why? Some users report that the chip returns 0 for both ASIC_ID and COMP_ID after power on, but then works fine after a reset. This may be a silicon bug or an issue with the power supply ramping up too slowly. In either case, treat this as a non-fatal error and allow the state machine to continue to the configuration step, where we will check the IDs again and reset if they are still wrong.
+    return ok;
+
+    //return success;
 }
 
 void AP_InertialSensor_SCH16T::configure_registers()
@@ -331,7 +792,14 @@ void AP_InertialSensor_SCH16T::configure_registers()
         register_write(r.addr, r.value);
     }
 
-    register_write(CTRL_USER_IF, DRY_DRV_EN); // Enable data ready
+    uint16_t reg_value;
+    reg_value = SPI48_DATA_UINT16(register_read(CTRL_USER_IF));
+    if (drdy_pin != 0) {
+        reg_value |= DRY_DRV_EN; // Enable data ready function on the DRDY pin
+    } else {
+        reg_value &= ~DRY_DRV_EN; // Disable data ready function, we will poll instead for now
+    }
+    register_write(CTRL_USER_IF, reg_value);
     register_write(CTRL_MODE, EN_SENSOR); // Enable the sensor
 }
 
@@ -380,7 +848,7 @@ void AP_InertialSensor_SCH16T::read_status_registers()
     _sensor_status.acc_z        = SPI48_DATA_UINT16(register_read(STAT_ACC_Z));
 }
 
-uint64_t AP_InertialSensor_SCH16T::register_read(uint8_t addr)
+inline uint64_t AP_InertialSensor_SCH16T::register_read(uint8_t addr)
 {
     uint64_t frame = {};
     frame |= uint64_t(addr) << 38; // Target address offset
@@ -426,7 +894,7 @@ uint64_t AP_InertialSensor_SCH16T::transfer_spi_frame(uint64_t frame)
     return value;
 }
 
-uint8_t AP_InertialSensor_SCH16T::calculate_crc8(uint64_t frame)
+inline uint8_t AP_InertialSensor_SCH16T::calculate_crc8(uint64_t frame)
 {
     uint64_t data = frame & 0xFFFFFFFFFF00LL;
     uint8_t crc = 0xFF;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
index 5dbff35955f2bb..8197d348f6ac4e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
@@ -24,7 +24,10 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
 public:
     static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
                                             AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                            enum Rotation rotation);
+                                            enum Rotation rotation,
+                                            uint8_t drdy_gpio,
+                                            uint8_t reset_gpio
+                                        );
 
     /**
      * Configure the sensors and start reading routine.
@@ -36,7 +39,10 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
 
     AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
                                 AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                enum Rotation rotation);
+                                enum Rotation rotation,
+                                uint8_t _drdy_pin,
+                                uint8_t reset_pin
+                            );
 
     struct SensorData {
         int32_t acc_x;
@@ -71,8 +77,18 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
         uint16_t value;
     };
 
+    struct ReadStatus {
+        //bool is_ok;
+        bool warn_no_new_sample;
+        bool err_sample_inconsistent;
+    };
+
+
     void run_state_machine(void);
-    bool collect_and_publish();
+    void run_state_machine_non_periodic();
+    void loop_thread(void);
+
+    bool collect_and_publish(bool use_thread, uint64_t sample_time_us, ReadStatus *read_status);
 
     void reset_chip();
     bool read_product_id();
@@ -81,7 +97,10 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
     void configure_registers();
     bool validate_register_configuration();
 
-    bool read_data(SensorData *data);
+    void perform_read();
+    inline bool validate_frame_errors(uint64_t value);
+    inline bool validate_received_frame(uint64_t value, int &last_dcnt, ReadStatus *read_status);
+    bool read_data(SensorData *data, ReadStatus *read_status);
 
     void register_write(uint8_t addr, uint16_t value);
     uint64_t register_read(uint8_t addr);
@@ -107,8 +126,31 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
     uint8_t gyro_instance = {};
     enum Rotation rotation {};
     uint8_t drdy_pin {};
+    uint8_t reset_pin {};
 
     int failure_count {};
+
+    int earlier_dcnt = -1;
+    uint64_t num_missed_samples;
+    uint64_t num_ok_samples;
+    uint64_t num_inconsistent_samples;
+    uint64_t num_no_new_samples;
+
+    uint64_t total_dt = 0;
+    uint32_t min_dt = 100000;
+    uint32_t max_dt = 0;
+    uint64_t num_dt_items = 0;
+
+    uint32_t max_reporting_dt = 0;
+    uint32_t max_read_data_dt = 0;
+    uint32_t max_read_spi_dt = 0;
+    uint32_t max_crc8_dt = 0;
+
+    uint64_t num_wait;
+    uint64_t total_wait_time_us;
+
+    uint64_t num_drydry_ok = 0;
+
     float expected_sample_rate_hz {};
 
     float accel_scale {};

From 1ba1de3e913f5c3debfbc29b8bd20fc8abbc059b Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Tue, 24 Mar 2026 21:37:17 +0200
Subject: [PATCH 15/19] nmea logging

---
 libraries/AP_GPS/AP_GPS_NMEA.cpp | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp
index b19d4012f48b28..b5e024f25bd1f8 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp
@@ -1037,7 +1037,8 @@ void AP_GPS_NMEA::log_GPS_Heading_pqtmtar()
 
     struct log_GPS_Heading pkt {
         LOG_PACKET_HEADER_INIT(LOG_GPS_Heading_MSG),
-        time_us                 : ph.time_ms * 1000, // convert from milliseconds to microseconds
+        // FIXME: dont i need autopilot time here from bootup?
+        time_us                 : AP_HAL::micros64(), //ph.time_ms * 1000, // convert from milliseconds to microseconds
         instance                : 0U,  // FIXME !? should be logged from elsewhere?
         quality                 : (uint8_t)ph.quality,
         baseline_length_m       : float_to_int16_100(ph.baseline_length),
@@ -1144,12 +1145,17 @@ void AP_GPS_NMEA::parse_pqtmtar_field(uint16_t term_number, const char *term)
                     ph.heading_acc);
         }
         // FIXME: should be quality=4 only
-        bool accept_heading_quality = ph.quality == 4; // only accept when we have RTK fix
+        bool rtk_fixed = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED == state.status;
+        bool accept_heading_accuracy = true && ph.heading_acc < 2.0f && ph.heading_acc > 0.001; // only accept if we have a valid accuracy estimate
+        bool accept_heading_quality = true && rtk_fixed && ph.quality == 4 && accept_heading_accuracy; // only accept when we have RTK fix
+        
         if (accept_heading_quality) {
             // FIXME: should use this only when quality = 4
             // have a valid RTK fixed heading solution
 
             // FIXME: some shared logic with UNIHEADINGA parsing...
+            // FIXME: should validate!
+            state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED;
             state.relPosHeading = ph.heading;
             state.relPosLength = ph.baseline_length;
             const float alt_diff = ph.baseline_length * tanf(radians(-ph.pitch));

From ab779b0fae66d65dd20353a748c4e8df6854cd9d Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Tue, 24 Mar 2026 21:42:52 +0200
Subject: [PATCH 16/19] sch16t kinda working

---
 .../hwdef/MicoAir743-Lite/hwdef.dat           |  10 +-
 .../AP_InertialSensor_SCH16T.cpp              | 253 +++++++++++++-----
 .../AP_InertialSensor_SCH16T.h                |   4 +-
 3 files changed, 199 insertions(+), 68 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
index 726339acc0f407..42d7c46b993f4a 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
@@ -170,10 +170,14 @@ undef PE5
 undef PE6
 #not undef PB14
 #not undef PB15
-undef PA7
-undef PA6
+
 undef PB1 # M5 used as SCH16 reset pin
 
+# undef PWM6-7-8
+undef PB0
+undef PA7 # PWM7
+undef PA6 # PWM8
+
 # SPI4 - External IMU (green LED channel approach)
 # Wiring:
 #   SCK  = PE2  (tap the MCU-side of the GREEN LED series resistor after removing it)
@@ -197,7 +201,7 @@ define SCH16T_RESET_PIN 54
 # 1*MHZ 2*MHZ is too slow
 # 10MHZ 10Mh` is failing ?
 # FIXME swicth to high speed mode later on? was 8 8
-SPIDEV  extsch16t     SPI4  DEVID1  SCH16T_CS     MODE0   2*MHZ 8*MHZ
+SPIDEV  extsch16t     SPI4  DEVID1  SCH16T_CS     MODE0   1*MHZ 8*MHZ
 
 # e.g.:
 #  IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
index 056ee176562d80..488abca3355ac8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -257,9 +257,13 @@ void AP_InertialSensor_SCH16T::start()
          */
         if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SCH16T::loop_thread, void),
                                           "SCH16T",
-                                          1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
+                                          8192,AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
             AP_HAL::panic("Failed to create SCH16T thread");
         }
+        hal.scheduler->delay(300); // give some time for the thread to start and gather some samples before we start the main loop
+        while(num_ok_samples < 100) {
+            hal.scheduler->delay(300);
+        }
     }
 
 }
@@ -267,8 +271,7 @@ void AP_InertialSensor_SCH16T::start()
 void sch16t_delay_us(uint32_t usec)
 {
     if (usec < 65535UL) {
-        hal.scheduler->delay_microseconds(usec);
-        return;
+        hal.scheduler->delay_microseconds((uint16_t) usec);
     } else {
         // for longer delays, we need to split into multiple calls, as it takes a uint16_t
         hal.scheduler->delay((uint16_t) (usec / (uint32_t)1000));
@@ -278,12 +281,13 @@ void sch16t_delay_us(uint32_t usec)
 
 
 void AP_InertialSensor_SCH16T::perform_read(){
-    // no semaphore here?
     const uint32_t period_us = (1000000UL / expected_sample_rate_hz) - 20U;
     ReadStatus read_status = {};
+    uint32_t last_reset_time = AP_HAL::micros();
     while(_state == State::Read) {
         uint32_t tstart = AP_HAL::micros();
-        WITH_SEMAPHORE(dev->get_semaphore());
+        bool collect_status = false;
+        
         uint32_t event_time = tstart;
         bool drdy_wait_ok = false;
         bool wait_ok = true;
@@ -296,8 +300,14 @@ void AP_InertialSensor_SCH16T::perform_read(){
             }
             event_time = AP_HAL::micros();
         }
+        
+        {
+            // minimize the time we hold the semaphore
+            // WITH_SEMAPHORE(dev->get_semaphore());
+            collect_status = collect_and_publish(true, event_time, &read_status);
+        }
         // FIMXE: getting huge variation in accel readings...
-        if (collect_and_publish(true, event_time, &read_status)) {
+        if (collect_status) {
             if (failure_count > 0) {
                 failure_count--;
             }
@@ -310,17 +320,58 @@ void AP_InertialSensor_SCH16T::perform_read(){
                 max_dt = max_dt < dt ? dt : max_dt;
             }
             num_ok_samples++;
-            // log status every few seconds
-            if (num_ok_samples % (1500*100) == 0) {
-                if (true) { 
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, 
-                        "SCH16T: %llu, %llu DRDY, %llu miss, %llu mixed, %llu no new, %d fail, avg_time=%llu us (min=%lu max=%lu, max report=%lu read=%lu crc8=%lu spi=%lu over %llu)",//, avg_wait=%llu us (over %llu)",
-                        num_ok_samples, num_drydry_ok, num_missed_samples, num_inconsistent_samples, num_no_new_samples, failure_count, 
-                        num_dt_items > 0 ? total_dt / num_dt_items : 0, min_dt, max_dt, max_reporting_dt, max_read_data_dt, max_crc8_dt, max_read_spi_dt, num_dt_items
-                        //num_wait > 0 ? total_wait_time_us / num_wait : 0, num_wait
-                    );
+        } else {
+            num_bad_samples++;
+            if (read_status.warn_no_new_sample) {
+                // we didn't get a new sample, but it's not an error, just wait for the next one
+                wait_ok = false;
+                num_no_new_samples++;
+                // read again almost immediately, expect that we'll get better alignment in time
+                // without delay here, sensor keeps restarting
+                if (drdy_pin == 0) {
+                    sch16t_delay_us(1);
                 }
+            } else {
+                // some other error, we should reset the sensor
+                failure_count++;
+            }
+        }
+
+        if (read_status.err_sample_inconsistent) {
+            // we got a sample but it was inconsistent (part of measurements is old), so we should try to read again immediately
+            // without waiting, as we might have just caught the sample in the middle of being updated
+            // and the next read might be consistent
+            wait_ok = false;
+            // if we have DRDY, do not restart on inconsistent sample, just wait for another DRDY
+            if (drdy_pin == 0){
+                failure_count++; 
+            }
+            num_inconsistent_samples++;
+            if (drdy_pin == 0) {
+                sch16t_delay_us(1);
+            }
+        }
+
+        // log status every few seconds
+        uint64_t num_samples = num_ok_samples + num_bad_samples;
+        if (num_samples % (1500*15) == 0) {
+            bool fishy = num_ok_samples != num_drydry_ok || num_missed_samples > 0 || num_no_new_samples > 0;
+            bool slightly_fishy = num_inconsistent_samples > 0;
+            bool max_counters = num_samples > 1500*60;
+            bool report_inconsistent_samples = slightly_fishy && max_counters; // 1min
+            bool reset_counters = fishy || max_counters;
+
+            if (fishy || report_inconsistent_samples) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, 
+                    "SCH16T: %llu, %llu DRDY, %llu miss, %llu mixed, %llu no new, %d fail, avg_time=%llu us (min=%lu max=%lu, max report=%lu read=%lu crc8=%lu spi=%lu over %llu)",//, avg_wait=%llu us (over %llu)",
+                    num_ok_samples, num_drydry_ok, num_missed_samples, num_inconsistent_samples, num_no_new_samples, failure_count, 
+                    num_dt_items > 0 ? total_dt / num_dt_items : 0, min_dt, max_dt, max_reporting_dt, max_read_data_dt, max_crc8_dt, max_read_spi_dt, num_dt_items
+                    //num_wait > 0 ? total_wait_time_us / num_wait : 0, num_wait
+                );
+            }
+            if (reset_counters) {
                 num_ok_samples = 0;
+                num_bad_samples = 0;
                 num_missed_samples = 0;
                 num_inconsistent_samples = 0;
                 num_no_new_samples = 0;
@@ -339,36 +390,19 @@ void AP_InertialSensor_SCH16T::perform_read(){
 
                 num_drydry_ok = 0;
             }
-        } else {
-            if (read_status.warn_no_new_sample) {
-                // we didn't get a new sample, but it's not an error, just wait for the next one
-                wait_ok = false;
-                num_no_new_samples++;
-                // read again almost immediately, expect that we'll get better alignment in time
-                // without delay here, sensor keeps restarting
-                if (drdy_pin == 0) {
-                    sch16t_delay_us(1);
-                }
-            } else if (read_status.err_sample_inconsistent) {
-                // we got a sample but it was inconsistent, so we should try to read again immediately
-                // without waiting, as we might have just caught the sample in the middle of being updated
-                // and the next read might be consistent
-                wait_ok = false;
-                failure_count++;
-                num_inconsistent_samples++;
-                if (drdy_pin == 0) {
-                    sch16t_delay_us(1);
-                }
-            } else {
-                // some other error, we should reset the sensor
-                failure_count++;
-            }
         }
 
         // Reset if successive failures
         if (failure_count > 100) {
-            _state = State::Reset;
-            return;
+            // do not reset more often than every few seconds
+            // important during startup
+            if (last_reset_time + 5000000UL < AP_HAL::micros()) {
+                last_reset_time = AP_HAL::micros();
+                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SCH16T: resetting");
+                _state = State::Reset;
+                return;
+            }
+            failure_count = 0;
         } else {
             if (wait_ok) {
                 uint32_t dt = AP_HAL::micros() - tstart;
@@ -376,8 +410,7 @@ void AP_InertialSensor_SCH16T::perform_read(){
                 num_dt_items++;
                 total_dt += dt;
                 
-                // FIXME: this is strangely sensitive to -20U value
-                            
+                // FIXME: this is strangely sensitive to -20U value          
                 if (dt < period_us) {
                     uint32_t wait_us = period_us - dt;
                     if (!drdy_wait_ok || wait_us >= 1U) {
@@ -394,66 +427,111 @@ void AP_InertialSensor_SCH16T::perform_read(){
 
 void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
 {
-
-    WITH_SEMAPHORE(dev->get_semaphore());
-
     // like run_state_machine, just uses delay instead of periodic callback, 
     // to be used if the driver is configured to run in a thread instead of using periodic callbacks
     switch (_state) {
     case State::PowerOn: {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron");
-            _state = State::Reset;
-            sch16t_delay_us(POWER_ON_TIME);
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron delay done");
+            bool next_step_reset = true;
+            if (next_step_reset) {
+                _state = State::Reset;
+                // at least if using external reset pin,
+                // no need to wait that long before initiating a reset?
+                if (reset_pin != 0) {
+                    sch16t_busywait_us(50*1000UL); // 1ms
+                } else {
+                    sch16t_busywait_us(POWER_ON_TIME);
+                }
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron delay done");
+            } else {
+                // skip reset and go straight to configuration, to save time on startup
+                // if it'll fail, it'll reset at the end of the "flow"
+                _state = State::Configure;
+                // 1ms voltage + 32ms NVM read and SPI setup
+                sch16t_busywait_us(RESET_TIME);
+            }
+
             break;
         }
 
     case State::Reset: {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: resetting");
+            //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: resetting");
             failure_count = 0;
-            reset_chip();
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                reset_chip();
+            }
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset done");
             _state = State::Configure;
+            //sch16t_busywait_us(RESET_TIME); // maybe this too short?
             sch16t_delay_us(POWER_ON_TIME);
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset delay done");
+            //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset done");
             break;
         }
     
     case State::Configure: {
-            if (!read_product_id()) {
+            bool product_id_ok = false;
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                product_id_ok = read_product_id();
+            }
+            
+            if (!product_id_ok) {
+                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SCH16T: Product ID bad, resetting");
                 _state = State::Reset;
                 sch16t_delay_us(2000000); // 2s
                 break;
+            } else {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: Prod ID ok");
             }
 
-            configure_registers();
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                configure_registers(); 
+                // writes EN_SENSOR
+                // datasheet: need to wait 215ms
+            }
             _state = State::LockConfiguration;
-            sch16t_delay_us(POWER_ON_TIME);
+            sch16t_busywait_us(POWER_ON_TIME);
             break;
         }
 
     case State::LockConfiguration: {
-            read_status_registers(); // Read all status registers once
-            register_write(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                read_status_registers(); // Read all status registers once
+                register_write(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR
+            }
             _state = State::Validate;
+             // datasheet: need to wait 3ms before validating registers
+            //sch16t_busywait_us(5000UL); 
             sch16t_delay_us(50000UL); // 50ms
             break;
         }
 
     case State::Validate: {
-            read_status_registers(); // Read all status registers twice
-            read_status_registers();
+            bool sensor_status_ok = false;
+            bool register_config_ok = false;
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                read_status_registers(); // Read all status registers twice
+                read_status_registers();
+                sensor_status_ok = validate_sensor_status();
+                // this reads registers so need to be inside the semaphore
+                register_config_ok = validate_register_configuration();
+            }
 
             // Check that registers are configured properly and that the sensor status is OK
-            if (validate_sensor_status() && validate_register_configuration()) {
+            if (sensor_status_ok && register_config_ok) {
                 _state = State::Read;
                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: validated, will read");
             } else {
+                GCS_SEND_TEXT(MAV_SEVERITY_WARNING, 
+                    "SCH16T: not validated (sensor %d, registers %d), resetting", 
+                    sensor_status_ok, register_config_ok);
                 _state = State::Reset;
-                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: failed to validate, resetting again");
-                sch16t_delay_us(POWER_ON_TIME);
+                sch16t_busywait_us(POWER_ON_TIME);
             }
-
             break;
         }
 
@@ -468,10 +546,21 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
 
 void AP_InertialSensor_SCH16T::loop_thread(void)
 {
+    
     while (true) {
         if (_state == State::Read) {
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+                //dev->set_speed(AP_HAL::Device::SPEED_LOW);
+
+            }
             perform_read();
         } else {
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                dev->set_speed(AP_HAL::Device::SPEED_LOW);
+            }
             run_state_machine_non_periodic();
         }
         //hal.scheduler->delay_microseconds(T_STALL_US);
@@ -600,6 +689,8 @@ void AP_InertialSensor_SCH16T::reset_chip()
         palSetLine(reset_pin);
     } else {
         register_write(CTRL_RESET, SPI_SOFT_RESET);
+        // SPI Communication is not allowed during 2ms after SPI SOFTRESET.
+        sch16t_delay_us(2*1000UL);
     }
 }
 
@@ -622,6 +713,9 @@ inline bool AP_InertialSensor_SCH16T::validate_frame_errors(uint64_t value){
 
 inline bool AP_InertialSensor_SCH16T::validate_received_frame(uint64_t value, int &last_dcnt, ReadStatus *read_status)
 {
+    // FIXME: shall we allow reporting samples where part of measurement is old?
+    // maybe it's OK at least during init when load on MCU is larger?
+    bool allow_inconsistent_sample = true;
     if (!validate_frame_errors(value)) {
         return false;
     }
@@ -634,7 +728,9 @@ inline bool AP_InertialSensor_SCH16T::validate_received_frame(uint64_t value, in
     if (last_dcnt != -1 && data_counter != last_dcnt) {
         read_status->warn_no_new_sample = false;
         read_status->err_sample_inconsistent = true;
-        return false;
+        if (!allow_inconsistent_sample) {
+            return false;
+        }
     }
 
     // check that we have a new sample, otherwise we are reading the same sample again, which is not good
@@ -707,6 +803,7 @@ bool AP_InertialSensor_SCH16T::read_data(SensorData *data, ReadStatus *read_stat
     if (!validate_received_frame(gyro_x, last_dcnt, read_status)) return false;
     if (!validate_received_frame(gyro_y, last_dcnt, read_status)) return false;
     if (!validate_received_frame(gyro_z, last_dcnt, read_status)) return false;
+    last_dcnt = -1; // ignore accel & gyro having different data counters ?
     if (!validate_received_frame(acc_x, last_dcnt, read_status)) return false;
     if (!validate_received_frame(acc_y, last_dcnt, read_status)) return false;
     if (!validate_received_frame(acc_z, last_dcnt, read_status)) return false;
@@ -794,6 +891,34 @@ void AP_InertialSensor_SCH16T::configure_registers()
 
     uint16_t reg_value;
     reg_value = SPI48_DATA_UINT16(register_read(CTRL_USER_IF));
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CTRL_USER_IF before DRDY: 0x%04X", reg_value);
+    // this has read 0000
+    // Maybe we need to reset the registers to defaults?
+
+    // [13:12] 
+    // Typical delay time of 1st level status clearance. When user reads data register, the
+    // associated 1st level status register is cleared after TDEL.
+    // 00 - 0.078 ms
+    // 01 - 0.625 ms
+    // 10 - 2.5 ms (default)
+    // 11 – 5 ms
+    #define FTREE_TDEL_DEFAULT (1 << 13) // 2.5ms (default)
+    reg_value |= FTREE_TDEL_DEFAULT;
+
+    // MISO_SR_CTRL MISO Slew Rate control [3:3] 
+    // 0 - SR control disabled without static current (fast rise/fall time ~<1ns). (Contact
+    // sales office before enabling)
+    // 1 - SR control enabled with static current (default)
+    #define MISO_SR_CTRL_DEFAULT (1 << 3)
+    reg_value |= MISO_SR_CTRL_DEFAULT;
+
+    // DRY_SR_CTRL DRY Slew Rate control [2:2]
+    // 0 - SR control disabled without static current (fast rise/fall time ~<1ns). (Contact
+    // sales office before enabling)
+    // 1 - SR control enabled with static current (default)
+    #define DRY_SR_CTRL_DEFAULT (1 << 2)
+    reg_value |= DRY_SR_CTRL_DEFAULT;
+
     if (drdy_pin != 0) {
         reg_value |= DRY_DRV_EN; // Enable data ready function on the DRDY pin
     } else {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
index 8197d348f6ac4e..4cf56f4ad52fae 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
@@ -132,7 +132,9 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
 
     int earlier_dcnt = -1;
     uint64_t num_missed_samples;
-    uint64_t num_ok_samples;
+    uint64_t num_ok_samples = 0;
+    uint64_t num_bad_samples = 0;
+
     uint64_t num_inconsistent_samples;
     uint64_t num_no_new_samples;
 

From 554e0be8f02f37022c3287e62a016c39620b8bbf Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Fri, 3 Apr 2026 16:33:40 +0300
Subject: [PATCH 17/19] sch16t k10 wip

---
 .../hwdef/MicoAir743-Lite/hwdef.dat           |  9 +--
 .../AP_InertialSensor_SCH16T.cpp              | 80 ++++++++++++++-----
 .../AP_InertialSensor_SCH16T.h                |  7 +-
 3 files changed, 71 insertions(+), 25 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
index 42d7c46b993f4a..7a71284f4a4a48 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
@@ -207,13 +207,12 @@ SPIDEV  extsch16t     SPI4  DEVID1  SCH16T_CS     MODE0   1*MHZ 8*MHZ
 #  IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
 # IMU <driver> SPI:ext_imu ROTATION_ROLL_180_YAW_90
 
-# FIXME no DRDY in current driver, compare driver with https://github.com/ArduPilot/ardupilot/commit/62727a05136948495325475be6a98ee3dbf1a085#diff-dc1e749347e6cf45fd4a92c928f301de27775c72e8e177a707315d099d2324a0
-#define EXT_IMU_DRDY_PIN ((uint8_t)62)
-#IMU SCH16T      SPI:extsch16t ROTATION_NONE EXT_IMU_DRDY_PIN
+define SCH16T_K01 1
+define SCH16T_K10 10
 
-IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN
+IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN SCH16T_K10
 
-#define AP_CLOCK_CORRECTION_PPM 1700
+define AP_CLOCK_CORRECTION_PPM 1700
 
 # hack the frequency to match PPS?
 #undef CH_CFG_ST_FREQUENCY
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
index 488abca3355ac8..f3102f78e1d8d2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -97,8 +97,11 @@ static constexpr uint16_t ACC12_8G_1475HZ    = 0b0001001011011011;  // Acc XYZ r
 
 
 static constexpr uint16_t ACC3_26G = (0b000 << 0); // Data is 3 bit, default. 
+// Writing 4b1010 to this field generates a SPI soft reset. SPI Communication is not
+// allowed during 2ms after SPI SOFTRESET.
 static constexpr uint16_t SPI_SOFT_RESET = (0b1010);
-static constexpr uint32_t POWER_ON_TIME = 250000UL;
+static constexpr uint32_t POWER_ON_TIME = 250000UL + 5000UL;
+static constexpr uint32_t RESET_TIME = 60000UL; // 33 ms max reset time according to datasheet, we wait more to be safe
 
 // Data registers
 #define RATE_X1         0x01 // 20 bit
@@ -158,12 +161,15 @@ AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
                                                          AP_HAL::OwnPtr<AP_HAL::Device> _dev,
                                                          enum Rotation _rotation,
                                                          uint8_t _drdy_pin,
-                                                        uint8_t _reset_pin)
+                                                         uint8_t _reset_pin,
+                                                         uint8_t _chip_variant
+                                                    )
     : AP_InertialSensor_Backend(imu)
     , dev(std::move(_dev))
     , rotation(_rotation)
     , drdy_pin(_drdy_pin)
-    , reset_pin(_reset_pin)
+    , reset_pin(0) // FIXME _reset_pin - forced not to use hw reset for now.
+    , chip_variant(_chip_variant)
 {
     // on K01:  according to datasheet,
     // the update rate is 23.6/X (1 kHz max) where X is the decimation factor set in the control registers. 
@@ -191,7 +197,22 @@ AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
     // if using ACC2 with 8G range, the sensitivity is 3200 LSB/(m/s^2), so accel_scale = 1.f / 3200.f;
     accel_scale = 1.f / 3200.f;
 
-    gyro_scale = radians(1.f / 1600.f);
+    // if K10 - 100, 200, 400 -> 200 @ 1.475kHz ODR, so gyro_scale = 1.f / 200.f;
+        // case 100:
+        //     return 0x02;   // 010
+        // case 200:
+        //     return 0x03;   // 011      
+        // case 400:
+        //     return 0x04;   // 100
+    if (chip_variant == 10) {
+        // 200 @ 1.475kHz ODR
+        // 200 LSB/(deg/s) ?, so gyro_scale = 1.f / 200.f;
+        gyro_scale = 1.f / 200.f;
+        //accel_scale = 1.f / 1600.f;
+    } else {
+        // if K01 - 1600 LSB/(deg/s), so  gyro_scale = 1.f / 1600.f;
+        gyro_scale = 1.f / 1600.f;
+    }
 
     _registers[0] = RegisterConfig(CTRL_FILT_RATE,  FILTER_BYPASS);
     _registers[1] = RegisterConfig(CTRL_FILT_ACC12, FILTER_BYPASS);
@@ -216,15 +237,17 @@ AP_InertialSensor_SCH16T::probe(AP_InertialSensor &imu,
                                    AP_HAL::OwnPtr<AP_HAL::Device> dev,
                                    enum Rotation rotation,
                                    uint8_t drdy_gpio,
-                                   uint8_t reset_gpio
+                                   uint8_t reset_gpio,
+                                   uint8_t _chip_variant
                                 )
 {
     if (!dev) {
         return nullptr;
     }
 
-    auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation, drdy_gpio, reset_gpio);
+    auto sensor = new AP_InertialSensor_SCH16T(imu, std::move(dev), rotation, drdy_gpio, reset_gpio, _chip_variant);
 
+    // FIXME: sensor should be validated here!!!
     if (!sensor) {
         return nullptr;
     }
@@ -303,7 +326,7 @@ void AP_InertialSensor_SCH16T::perform_read(){
         
         {
             // minimize the time we hold the semaphore
-            // WITH_SEMAPHORE(dev->get_semaphore());
+            WITH_SEMAPHORE(dev->get_semaphore());
             collect_status = collect_and_publish(true, event_time, &read_status);
         }
         // FIMXE: getting huge variation in accel readings...
@@ -438,9 +461,9 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
                 // at least if using external reset pin,
                 // no need to wait that long before initiating a reset?
                 if (reset_pin != 0) {
-                    sch16t_busywait_us(50*1000UL); // 1ms
+                    sch16t_delay_us(50*1000UL); // 1ms
                 } else {
-                    sch16t_busywait_us(POWER_ON_TIME);
+                    sch16t_delay_us(POWER_ON_TIME);
                 }
                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: poweron delay done");
             } else {
@@ -448,7 +471,7 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
                 // if it'll fail, it'll reset at the end of the "flow"
                 _state = State::Configure;
                 // 1ms voltage + 32ms NVM read and SPI setup
-                sch16t_busywait_us(RESET_TIME);
+                sch16t_delay_us(RESET_TIME);
             }
 
             break;
@@ -463,7 +486,6 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
             }
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset done");
             _state = State::Configure;
-            //sch16t_busywait_us(RESET_TIME); // maybe this too short?
             sch16t_delay_us(POWER_ON_TIME);
             //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: reset done");
             break;
@@ -492,7 +514,7 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
                 // datasheet: need to wait 215ms
             }
             _state = State::LockConfiguration;
-            sch16t_busywait_us(POWER_ON_TIME);
+            sch16t_delay_us(POWER_ON_TIME);
             break;
         }
 
@@ -504,7 +526,6 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
             }
             _state = State::Validate;
              // datasheet: need to wait 3ms before validating registers
-            //sch16t_busywait_us(5000UL); 
             sch16t_delay_us(50000UL); // 50ms
             break;
         }
@@ -530,7 +551,7 @@ void AP_InertialSensor_SCH16T::run_state_machine_non_periodic()
                     "SCH16T: not validated (sensor %d, registers %d), resetting", 
                     sensor_status_ok, register_config_ok);
                 _state = State::Reset;
-                sch16t_busywait_us(POWER_ON_TIME);
+                sch16t_delay_us(POWER_ON_TIME);
             }
             break;
         }
@@ -685,7 +706,8 @@ void AP_InertialSensor_SCH16T::reset_chip()
 {
     if (reset_pin != 0) {
         palClearLine(reset_pin);
-        hal.scheduler->delay(2000);
+        //hal.scheduler->delay(2000);
+        sch16t_delay_us(40*1000UL);
         palSetLine(reset_pin);
     } else {
         register_write(CTRL_RESET, SPI_SOFT_RESET);
@@ -781,6 +803,7 @@ bool AP_InertialSensor_SCH16T::read_data(SensorData *data, ReadStatus *read_stat
 
     // Check errors & Validate data counter early on, so we can read again later with minimum delay
     int last_dcnt = -1;
+    // FIXME: is it better to first read gyro or accel?
     register_read(RATE_X2);
     uint64_t gyro_x = register_read(RATE_Y2);
     //if (!validate_received_frame(gyro_x, last_dcnt, read_status)) return false;
@@ -794,6 +817,8 @@ bool AP_InertialSensor_SCH16T::read_data(SensorData *data, ReadStatus *read_stat
     //if (!validate_received_frame(acc_y, last_dcnt, read_status)) return false;
     uint64_t acc_z  = register_read(TEMP);
     //if (!validate_received_frame(acc_z, last_dcnt, read_status)) return false;
+
+    // FIXME: could report temperature at lower rate to save some SPI time... ?
     uint64_t temp   = register_read(TEMP);
 
     uint32_t read_spi_end = AP_HAL::micros();
@@ -803,7 +828,12 @@ bool AP_InertialSensor_SCH16T::read_data(SensorData *data, ReadStatus *read_stat
     if (!validate_received_frame(gyro_x, last_dcnt, read_status)) return false;
     if (!validate_received_frame(gyro_y, last_dcnt, read_status)) return false;
     if (!validate_received_frame(gyro_z, last_dcnt, read_status)) return false;
-    last_dcnt = -1; // ignore accel & gyro having different data counters ?
+    // FIXME: it seems DATA_COUNTER (DCNT) probably should be the same for gyro and accel,
+    // but most time I was getting different data counter for gyro and accel,
+    // though within gyro, or within the accel, the data counter was consistent. 
+    // So for now I'm checking data counter consistency only within gyro and accel.
+    // Is this due to scheduler/interrupt delay after DRY? or IMU config issue? 
+    last_dcnt = -1;
     if (!validate_received_frame(acc_x, last_dcnt, read_status)) return false;
     if (!validate_received_frame(acc_y, last_dcnt, read_status)) return false;
     if (!validate_received_frame(acc_z, last_dcnt, read_status)) return false;
@@ -875,12 +905,23 @@ bool AP_InertialSensor_SCH16T::read_product_id()
     //bool success = asic_id == 0x20 && comp_id == 0x17;
     // asic_id - ASIC revision
     // comp_id - Component type
+
     bool ok = asic_id == 0x21 || comp_id == 0x23;
+    // FIXME: could also auto-detect the variant here based on the IDs, but for now passing _chip_variant from the constructor (hwdef)
+    if ((asic_id == 0x21 && comp_id == 0x23) || (asic_id == 0x20 && comp_id == 0x17)) {
+		// ASIC_ID = 0x21, COMP_ID = 0x23 is a K01 variant of REV_1
+		// ASIC_ID = 0x20, COMP_ID = 0x17 is a B13 variant of REV_1
+		//_detected_version = ChipVersion::REV_1;
+        ok = true;
+	} else if ((asic_id == 0x21 && comp_id == 0x24) || (asic_id == 0x21 && comp_id == 0x21)) {
+		// ASIC_ID = 0x21, COMP_ID = 0x24 is a B10 variant of REV_2
+		// ASIC_ID = 0x21, COMP_ID = 0x21 is a production K10 variant of REV_2
+		//_detected_version = ChipVersion::REV_2;
+        ok = true;
+    }
 
     // FIXME why? Some users report that the chip returns 0 for both ASIC_ID and COMP_ID after power on, but then works fine after a reset. This may be a silicon bug or an issue with the power supply ramping up too slowly. In either case, treat this as a non-fatal error and allow the state machine to continue to the configuration step, where we will check the IDs again and reset if they are still wrong.
     return ok;
-
-    //return success;
 }
 
 void AP_InertialSensor_SCH16T::configure_registers()
@@ -892,9 +933,12 @@ void AP_InertialSensor_SCH16T::configure_registers()
     uint16_t reg_value;
     reg_value = SPI48_DATA_UINT16(register_read(CTRL_USER_IF));
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CTRL_USER_IF before DRDY: 0x%04X", reg_value);
+
     // this has read 0000
     // Maybe we need to reset the registers to defaults?
 
+    // Force/reset default IMU config values (according to K01 datasheet):
+
     // [13:12] 
     // Typical delay time of 1st level status clearance. When user reads data register, the
     // associated 1st level status register is cleared after TDEL.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
index 4cf56f4ad52fae..9d5d1bf6b5e672 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
@@ -26,7 +26,8 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
                                             AP_HAL::OwnPtr<AP_HAL::Device> dev,
                                             enum Rotation rotation,
                                             uint8_t drdy_gpio,
-                                            uint8_t reset_gpio
+                                            uint8_t reset_gpio,
+                                            uint8_t _chip_variant
                                         );
 
     /**
@@ -41,7 +42,8 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
                                 AP_HAL::OwnPtr<AP_HAL::Device> dev,
                                 enum Rotation rotation,
                                 uint8_t _drdy_pin,
-                                uint8_t reset_pin
+                                uint8_t reset_pin,
+                                uint8_t _chip_variant
                             );
 
     struct SensorData {
@@ -127,6 +129,7 @@ class AP_InertialSensor_SCH16T : public AP_InertialSensor_Backend {
     enum Rotation rotation {};
     uint8_t drdy_pin {};
     uint8_t reset_pin {};
+    uint8_t chip_variant {}; // 1 - K01, 10 - K10, 20 - K20
 
     int failure_count {};
 

From e1a37278385fc939d94f0135b8f674980ee3f08f Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Fri, 3 Apr 2026 16:47:44 +0300
Subject: [PATCH 18/19] update Pixhawk6X example

---
 config_and_rebuild.sh                              | 5 +++++
 libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat | 6 +++++-
 2 files changed, 10 insertions(+), 1 deletion(-)
 create mode 100644 config_and_rebuild.sh

diff --git a/config_and_rebuild.sh b/config_and_rebuild.sh
new file mode 100644
index 00000000000000..699e4e5f2cb1bb
--- /dev/null
+++ b/config_and_rebuild.sh
@@ -0,0 +1,5 @@
+# git clone --recurse-submodules git@github.com:vidma/ardupilot.git
+#./waf configure --board=MambaF405-2022 && ./waf clean && ./waf plane
+
+# ./waf configure --board=Pixhawk6X && ./waf clean && ./waf plane
+./waf configure --board=MicoAir743-Lite && ./waf clean && ./waf plane
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
index 9c096fe6c0773e..ae8828c470d570 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat
@@ -358,7 +358,11 @@ IMU Invensensev3 SPI:icm45686-1  ROTATION_YAW_90    BOARD_MATCH(FMUV6_BOARD_HOLY
 # FIXME implement DRDY
 define ADIS_DRDY_PIN 93
 #PD11 DRDY_ADIS16507 INPUT GPIO(93)
-IMU SCH16T      SPI:SCH16T ROTATION_NONE #ADIS_DRDY_PIN
+#IMU SCH16T      SPI:SCH16T ROTATION_NONE #ADIS_DRDY_PIN
+
+define SCH16T_K10 10
+define SCH16T_RESET_PIN 0
+IMU SCH16T      SPI:SCH16T ROTATION_YAW_180 ADIS_DRDY_PIN SCH16T_RESET_PIN SCH16T_K10
 
 
 define HAL_INS_HIGHRES_SAMPLE 7

From 6e675dbec6062a05323dc55c64e0bffa580b5a34 Mon Sep 17 00:00:00 2001
From: vidmantas zemleris <vidma@kensu.io>
Date: Fri, 3 Apr 2026 17:11:06 +0300
Subject: [PATCH 19/19] upd

---
 libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat | 3 ++-
 libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
index 7a71284f4a4a48..d38a04dd65cfe4 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MicoAir743-Lite/hwdef.dat
@@ -196,6 +196,7 @@ define SCH16T_DRDY_PIN 57
 
 PB1 SCH16T_RESET  OUTPUT HIGH GPIO(54) # M5
 define SCH16T_RESET_PIN 54
+define SCH16T_RESET_PIN_DISABLED 0
 
 
 # 1*MHZ 2*MHZ is too slow
@@ -210,7 +211,7 @@ SPIDEV  extsch16t     SPI4  DEVID1  SCH16T_CS     MODE0   1*MHZ 8*MHZ
 define SCH16T_K01 1
 define SCH16T_K10 10
 
-IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN SCH16T_K10
+IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN_DISABLED SCH16T_K10
 
 define AP_CLOCK_CORRECTION_PPM 1700
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
index f3102f78e1d8d2..e1aa902234461e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -168,7 +168,7 @@ AP_InertialSensor_SCH16T::AP_InertialSensor_SCH16T(AP_InertialSensor &imu,
     , dev(std::move(_dev))
     , rotation(_rotation)
     , drdy_pin(_drdy_pin)
-    , reset_pin(0) // FIXME _reset_pin - forced not to use hw reset for now.
+    , reset_pin(_reset_pin)
     , chip_variant(_chip_variant)
 {
     // on K01:  according to datasheet,