diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index d98b38533afce..2173f35835142 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -458,6 +458,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/decode_devid.py b/Tools/scripts/decode_devid.py
index 3a432403e4be5..f9abe1a910df7 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",
+    0x3E : "DEVTYPE_INS_SCH16T",
 }
 
 baro_types = {
diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index b60adeca03ea8..6d81892f9a0ea 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -82,6 +82,12 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED', 'AP_RangeFinder_LightWareGRF::get_reading'),
 
             ('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_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/config_and_rebuild.sh b/config_and_rebuild.sh
new file mode 100644
index 0000000000000..699e4e5f2cb1b
--- /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_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index c569798cb775c..37ecea118aeb2 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)) {
@@ -1950,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 2bae27c6374bc..168a4c05e17cb 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
@@ -791,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 b61b6e1d1a3be..b5e024f25bd1f 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,30 @@ 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;
+        }
+        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
           ID. The most common is 'GP' but there are a bunch of others
@@ -670,6 +737,27 @@ 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;
+        #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
         }
     }
 
@@ -794,6 +882,313 @@ 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));
+        break;
+    case 2: // Build date (BuildDate)
+        strncpy(pv.build_date, term, sizeof(pv.build_date));
+        break;
+    case 3: // Build time (BuildTime)
+        strncpy(pv.build_time, term, sizeof(pv.build_time));
+        _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;
+    }
+}
+
+#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),
+        // 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),
+        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.
+
+  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);
+        // ratelimit output
+        const uint32_t now = AP_HAL::millis();
+        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:%d 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 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));
+            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: 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;
+    }
+}
+#endif // GPS_MOVING_BASELINE
+
+#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 +1236,22 @@ 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);
+
+    #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;
@@ -902,6 +1313,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 +1387,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,7 +1429,37 @@ 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
+#endif // HAL_LOGGING_ENABLED
+
+
+#ifdef HAL_GPIO_PPS
+void
+AP_GPS_NMEA::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
+{
+    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
 
 #endif // AP_GPS_NMEA_ENABLED
diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h
index 3109600b4ef8a..ea485c406556c 100644
--- a/libraries/AP_GPS/AP_GPS_NMEA.h
+++ b/libraries/AP_GPS/AP_GPS_NMEA.h
@@ -89,6 +89,11 @@ 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_PQTMTAR = 380, // extension for Quectel, heading info, 12 fields
         _GPS_SENTENCE_OTHER = 0
     };
 
@@ -146,6 +151,18 @@ 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);
+#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
     uint32_t _crc32;                                            ///< CRC for unicore messages
@@ -257,10 +274,85 @@ 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;
+
+    #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;
+    #if HAL_LOGGING_ENABLED
+    void log_GPS_Heading_pqtmtar();
+    #endif // HAL_LOGGING_ENABLED
+    #endif // GPS_MOVING_BASELINE
+
+    uint32_t _last_PQTM_vel_ms;
+    uint32_t _last_PQTM_acc_ms;
+    bool _expect_pqtm;
+#endif  // AP_GPS_NMEA_QUECTEL_ENABLED
+
     bool _expect_agrica;
 
     // last time we sent type specific config strings
     uint32_t last_config_ms;
+    uint32_t _last_pqmtar_log_ms = 0;
+
+    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_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h
index ad6b5364a29df..66c005c533457 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
diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
index 9296ec5f07201..7b10e0eaceb6d 100644
--- a/libraries/AP_GPS/LogStructure.h
+++ b/libraries/AP_GPS/LogStructure.h
@@ -6,9 +6,11 @@
 #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,                           \
+    LOG_GPS_Heading_MSG,                        \
     LOG_GPS_UBX1_MSG,                           \
     LOG_GPS_UBX2_MSG,                           \
     LOG_IDS_FROM_GPS_SBP
@@ -83,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
  */
@@ -208,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), \
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
index 035cac5bbbfff..293246674c9aa 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405-2022/hwdef.dat
@@ -34,3 +34,18 @@ 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
+
+# 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
+
+//define _AP_GPS_DEBUG_LOGGING_ENABLED 0
\ No newline at end of file
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 9df29b412a59a..a9091a21edc63 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 24f80c471bfe8..d38a04dd65cfe 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
@@ -152,3 +152,76 @@ 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
+
+
+# SCH16T and fake external SPI
+undef PE2
+undef PE5
+undef PE6
+#not undef PB14
+#not undef PB15
+
+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)
+#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
+define SCH16T_RESET_PIN_DISABLED 0
+
+
+# 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   1*MHZ 8*MHZ
+
+# e.g.:
+#  IMU Invensensev3 SPI:icm45686 ROTATION_ROLL_180_YAW_90
+# IMU <driver> SPI:ext_imu ROTATION_ROLL_180_YAW_90
+
+define SCH16T_K01 1
+define SCH16T_K10 10
+
+IMU SCH16T      SPI:extsch16t ROTATION_YAW_180 SCH16T_DRDY_PIN SCH16T_RESET_PIN_DISABLED SCH16T_K10
+
+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 396599565520f..ae8828c470d57 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,19 @@ 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 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
 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 f1e510ccebd07..9fdd3af0a6cf1 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.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 6c962e3b145c1..276a561f7fca8 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 75267f6afa64d..375275044ed90 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 0000000000000..e1aa902234461
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.cpp
@@ -0,0 +1,1084 @@
+/*
+ * 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
+
+
+// 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. 
+// 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 + 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
+#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,
+                                                         uint8_t _drdy_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)
+    , 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. 
+    // "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;
+
+    // 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);
+    _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,
+                                   uint8_t drdy_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, _chip_variant);
+
+    // FIXME: sensor should be validated here!!!
+    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;
+
+    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",
+                                          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);
+        }
+    }
+
+}
+
+void sch16t_delay_us(uint32_t usec)
+{
+    if (usec < 65535UL) {
+        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));
+        hal.scheduler->delay_microseconds((uint16_t) (usec % (uint32_t)1000));
+    }
+}
+
+
+void AP_InertialSensor_SCH16T::perform_read(){
+    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();
+        bool collect_status = false;
+        
+        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();
+        }
+        
+        {
+            // 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_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++;
+        } 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;
+
+                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;
+            }
+        }
+
+        // Reset if successive failures
+        if (failure_count > 100) {
+            // 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;
+                // 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()
+{
+    // 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");
+            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_delay_us(50*1000UL); // 1ms
+                } else {
+                    sch16t_delay_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_delay_us(RESET_TIME);
+            }
+
+            break;
+        }
+
+    case State::Reset: {
+            //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SCH16T: resetting");
+            failure_count = 0;
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                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 done");
+            break;
+        }
+    
+    case State::Configure: {
+            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");
+            }
+
+            {
+                WITH_SEMAPHORE(dev->get_semaphore());
+                configure_registers(); 
+                // writes EN_SENSOR
+                // datasheet: need to wait 215ms
+            }
+            _state = State::LockConfiguration;
+            sch16t_delay_us(POWER_ON_TIME);
+            break;
+        }
+
+    case State::LockConfiguration: {
+            {
+                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_delay_us(50000UL); // 50ms
+            break;
+        }
+
+    case State::Validate: {
+            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 (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;
+                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) {
+            {
+                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);
+    }
+}
+
+
+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;
+            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(false, AP_HAL::micros(), &read_status)) {
+                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(bool use_thread, uint64_t sample_time_us, ReadStatus *read_status)
+{
+    SensorData 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, sample_time_us);
+
+        _rotate_and_correct_gyro(gyro_instance, gyro);
+        _notify_new_gyro_raw_sample(gyro_instance, gyro, sample_time_us);
+
+        _publish_temperature(accel_instance, float(data.temp)/100.f);
+        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;
+}
+
+void AP_InertialSensor_SCH16T::reset_chip()
+{
+    if (reset_pin != 0) {
+        palClearLine(reset_pin);
+        //hal.scheduler->delay(2000);
+        sch16t_delay_us(40*1000UL);
+        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);
+    }
+}
+
+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)
+{
+    // 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;
+    }
+
+    // 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;
+        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
+    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;
+    // 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;
+    uint64_t gyro_y = register_read(RATE_Z2);
+    //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;
+
+    // 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();
+    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;
+
+    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;
+    // 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;
+
+
+    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);
+    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;
+
+    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;
+}
+
+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 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;
+    // 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;
+}
+
+void AP_InertialSensor_SCH16T::configure_registers()
+{
+    for (auto &r : _registers) {
+        register_write(r.addr, r.value);
+    }
+
+    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.
+    // 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 {
+        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
+}
+
+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));
+}
+
+inline 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;
+}
+
+inline 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 0000000000000..9d5d1bf6b5e67
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SCH16T.h
@@ -0,0 +1,163 @@
+/*
+ * 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,
+                                            uint8_t drdy_gpio,
+                                            uint8_t reset_gpio,
+                                            uint8_t _chip_variant
+                                        );
+
+    /**
+     * 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,
+                                uint8_t _drdy_pin,
+                                uint8_t reset_pin,
+                                uint8_t _chip_variant
+                            );
+
+    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;
+    };
+
+    struct ReadStatus {
+        //bool is_ok;
+        bool warn_no_new_sample;
+        bool err_sample_inconsistent;
+    };
+
+
+    void run_state_machine(void);
+    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();
+    void read_status_registers();
+    bool validate_sensor_status();
+    void configure_registers();
+    bool validate_register_configuration();
+
+    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);
+    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 {};
+    uint8_t reset_pin {};
+    uint8_t chip_variant {}; // 1 - K01, 10 - K10, 20 - K20
+
+    int failure_count {};
+
+    int earlier_dcnt = -1;
+    uint64_t num_missed_samples;
+    uint64_t num_ok_samples = 0;
+    uint64_t num_bad_samples = 0;
+
+    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 {};
+    float gyro_scale {};
+};