WIP: ODOMETRY quality metric

WIP
MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME
2026-05-23 20:27:35 +08:00 · 2022-06-29 10:16:41 -04:00 · 2022-06-28 21:23:00 -04:00 · 2022-06-24 19:44:43 +02:00 · 2022-06-24 10:05:16 -05:00 · 2022-06-24 10:05:16 -05:00
249 changed files with 11469 additions and 3452 deletions
@@ -86,10 +86,28 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial

 # Check for flow sensor
-if param compare SENS_EN_PX4FLOW 1
+if param compare -s SENS_EN_PX4FLOW 1
 then
 	px4flow start -X &
 fi

+if param compare -s IMU_GYRO_CAL_EN 1
+then
+	gyro_calibration start
+fi
+
+
+if param compare -s MBE_ENABLE 1
+then
+	# conservative mag bias estimation
+	param set-default MBE_LEARN_GAIN 5
+	param set-default IMU_GYRO_CUTOFF 20
+	mag_bias_estimator start
+fi
+
+param set-default SENS_MAG_RATE 100
+
+sensors start
+
 uavcannode start
 unset R
@@ -18,3 +18,7 @@ param set-default LPE_FAKE_ORIGIN 1

 param set-default MPC_ALT_MODE 2

+param set-default SENS_FLOW_ROT 6
+param set-default SENS_FLOW_MINHGT 0.7
+param set-default SENS_FLOW_MAXHGT 3.0
+param set-default SENS_FLOW_MAXR 2.5
@@ -9,7 +9,6 @@

 # EKF2
 param set-default EKF2_AID_MASK 2
-param set-default SENS_FLOW_ROT 0

 # LPE: Flow-only mode
 param set-default LPE_FUSION 242
@@ -62,7 +62,7 @@ param set-default MPC_JERK_AUTO 4
 param set-default MPC_LAND_SPEED 1
 param set-default MPC_MAN_TILT_MAX 25
 param set-default MPC_MAN_Y_MAX 40
-param set-default MPC_SPOOLUP_TIME 1.5
+param set-default COM_SPOOLUP_TIME 1.5
 param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TKO_RAMP_T 1.8
@@ -104,7 +104,6 @@ param set-default SDLOG_PROFILE 131
 param set-default SENS_CM8JL65_CFG 104
 param set-default SENS_FLOW_MAXHGT 25
 param set-default SENS_FLOW_MINHGT 0.5
-param set-default SENS_FLOW_ROT 0
 param set-default IMU_GYRO_CUTOFF 100
 param set-default SENS_EN_PMW3901 1

@@ -47,6 +47,7 @@ param set-default COM_RC_LOSS_T 3

 # ekf2
 param set-default EKF2_AID_MASK 33
+param set-default EKF2_TERR_MASK 1
 param set-default EKF2_BARO_DELAY 0
 param set-default EKF2_BARO_NOISE 2.0

@@ -173,7 +174,7 @@ param set-default RC1_TRIM          1000
 param set-default SENS_FLOW_MAXR   7.4
 param set-default SENS_FLOW_MINHGT 0.15
 param set-default SENS_FLOW_MAXHGT 5.0
-param set-default SENS_FLOW_ROT    0
+param set-default SENS_FLOW_ROT 0

 # ignore the SD card errors and use normal startup sound
 set STARTUP_TUNE "1"
@@ -1,72 +0,0 @@
-#!/bin/python3
-
-import parse_cmake.parsing as cmp
-import glob
-import pprint
-import re
-import os
-
-__location__ = os.path.realpath(
-    os.path.join(os.getcwd(), os.path.dirname(__file__)))
-
-serial_regex = r"(\D\D\D\d):(/dev/ttyS\d+)"
-io_regex = r"IO (.*)"
-romfs_regex = r"ROMFSROOT (.*)"
-arch_regex = r"ARCHITECTURE (.*)"
-toolchain_regex = r"TOOLCHAIN (.*)"
-
-
-
-def stripComments(code):
-    code = str(code)
-    return re.sub(r'(?m) *#.*\n?', '', code)
-
-lut = {}
-with open(os.path.join(__location__, "cmake_kconfig_lut.txt"),'r') as lookup:
-    for line in lookup:
-        if ',' in line:
-            key, value = line.strip().split(',')
-            lut[key] = value
-
-#for name in glob.glob('boards/*/*/*.cmake'):
-px4_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../'))
-
-for name in glob.glob(px4_dir + '/boards/*/*/*.cmake'):
-    print(name)
-    with open(name, 'r') as f:
-        romfs_set = False
-        w = open(name.replace(".cmake",".px4board"), "w")
-        for line in f:
-            clean_line = stripComments(line.strip())
-            value = lut.get(clean_line)
-            if value is not None:
-                print(value, file=w)
-                print(value)
-            else:
-                matches = re.finditer(serial_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"")
-                    print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"", file=w)
-                matches = re.finditer(io_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"", file=w)
-                matches = re.finditer(romfs_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"", file=w)
-                    romfs_set = True
-                matches = re.finditer(arch_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"", file=w)
-                matches = re.finditer(toolchain_regex, clean_line, re.MULTILINE)
-                for matchNum, match in enumerate(matches, start=1):
-                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"")
-                    print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"", file=w)
-
-    if(romfs_set == False):
-        print("CONFIG_BOARD_ROMFSROOT=\"\"", file=w)
-
-
-    w.close()
@@ -1,204 +0,0 @@
-PLATFORM nuttx,CONFIG_PLATFORM_NUTTX=y
-PLATFORM posix,CONFIG_PLATFORM_POSIX=y
-CONSTRAINED_MEMORY,CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONSTRAINED_FLASH,CONFIG_BOARD_CONSTRAINED_FLASH=y
-NO_HELP,CONFIG_BOARD_NO_HELP=y
-EXTERNAL_METADATA,CONFIG_BOARD_EXTERNAL_METADATA=y
-BUILD_BOOTLOADER,CONFIG_BOARD_BUILD_BOOTLOADER=y
-UAVCAN_INTERFACES 2,CONFIG_BOARD_UAVCAN_INTERFACES=2
-UAVCAN_INTERFACES 1,CONFIG_BOARD_UAVCAN_INTERFACES=1
-UAVCAN_TIMER_OVERRIDE 2,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=1
-UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=0
-TESTING,CONFIG_BOARD_TESTING=y
-ETHERNET,CONFIG_BOARD_ETHERNET=y
-adc/ads1115,CONFIG_DRIVERS_ADC_ADS1115=y
-adc/board_adc,CONFIG_DRIVERS_ADC_BOARD_ADC=y
-barometer,CONFIG_COMMON_BAROMETERS=y
-barometer/bmp280,CONFIG_DRIVERS_BAROMETER_BMP280=y
-barometer/bmp388,CONFIG_DRIVERS_BAROMETER_BMP388=y
-barometer/dps310,CONFIG_DRIVERS_BAROMETER_DPS310=y
-barometer/lps22hb,CONFIG_DRIVERS_BAROMETER_LPS22HB=y
-barometer/lps25h,CONFIG_DRIVERS_BAROMETER_LPS25H=y
-barometer/lps33hw,CONFIG_DRIVERS_BAROMETER_LPS33HW=y
-barometer/mpl3115a2,CONFIG_DRIVERS_BAROMETER_MPL3115A2=y
-barometer/ms5611,CONFIG_DRIVERS_BAROMETER_MS5611=y
-barometer/tcbp001ta,CONFIG_DRIVERS_BAROMETER_TCBP001TA=y
-batt_smbus,CONFIG_DRIVERS_BATT_SMBUS=y
-bootloaders,CONFIG_DRIVERS_BOOTLOADERS=y
-camera_capture,CONFIG_DRIVERS_CAMERA_CAPTURE=y
-camera_trigger,CONFIG_DRIVERS_CAMERA_TRIGGER=y
-differential_pressure,CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE=y
-distance_sensor,CONFIG_COMMON_DISTANCE_SENSOR=y
-distance_sensor/ll40ls,CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
-distance_sensor/lightware_laser_serial,CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
-distance_sensor/broadcom/afbrs50,CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
-distance_sensor/vl53l0x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
-distance_sensor/vl53l1x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
-distance_sensor/srf05,CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
-dshot,CONFIG_DRIVERS_DSHOT=y
-gps,CONFIG_DRIVERS_GPS=y
-heater,CONFIG_DRIVERS_HEATER=y
-imu,CONFIG_COMMON_IMU=y
-imu/adis16477,CONFIG_DRIVERS_IMU_ADIS16477=y
-imu/adis16497,CONFIG_DRIVERS_IMU_ADIS16497=y
-imu/analog_devices/adis16448,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
-imu/analog_devices/adis16470,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
-imu/bosch/bmi055,CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
-imu/bosch/bmi088,CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-imu/fxas21002c,CONFIG_DRIVERS_IMU_FXAS21002C=y
-imu/fxos8701cq,CONFIG_DRIVERS_IMU_FXOS8701CQ=y
-imu/invensense/icm20602,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-imu/invensense/icm20608g,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y
-imu/invensense/icm20649,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-imu/invensense/icm20689,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
-imu/invensense/icm20948,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-imu/invensense/icm40609d,CONFIG_DRIVERS_IMU_INVENSENSE_ICM40609D=y
-imu/invensense/icm42605,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
-imu/invensense/icm42688p,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-imu/invensense/mpu6000,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-imu/invensense/mpu6500,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y
-imu/invensense/mpu9250,CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
-imu/l3gd20,CONFIG_DRIVERS_IMU_L3GD20=y
-imu/lsm303d,CONFIG_DRIVERS_IMU_LSM303D=y
-imu/st,CONFIG_DRIVERS_IMU_ST=y
-irlock,CONFIG_DRIVERS_IRLOCK=y
-lights,CONFIG_COMMON_LIGHT=y
-lights/neopixel,CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y
-lights/rgbled,CONFIG_DRIVERS_LIGHTS_RGBLED=y
-lights/rgbled_ncp5623c,CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
-lights/rgbled_pwm,CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
-magnetometer,CONFIG_COMMON_MAGNETOMETER=y
-magnetometer/akm/ak09916,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y
-magnetometer/akm/ak8963,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y
-magnetometer/bosch/bmm150,CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-magnetometer/hmc5883,CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
-magnetometer/isentek/ist8308,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
-magnetometer/isentek/ist8310,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
-magnetometer/lis2mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y
-magnetometer/lis3mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
-magnetometer/lsm303agr,CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y
-magnetometer/lsm9ds1_mag,CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y
-magnetometer/qmc5883l,CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
-magnetometer/rm3100,CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
-magnetometer/vtrantech/vcm1193l,CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y
-optical_flow,CONFIG_COMMON_OPTICAL_FLOW=y
-optical_flow/paw3902,CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
-optical_flow/paw3901,CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y
-optical_flow/px4flow,CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y
-optical_flow/thoneflow,CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y
-osd,CONFIG_DRIVERS_OSD=y
-pca9685,CONFIG_DRIVERS_PCA9685=y
-pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y
-power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y
-power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
-pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y
-protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y
-pwm_input,CONFIG_DRIVERS_PWM_INPUT=y
-pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y
-pwm_out,CONFIG_DRIVERS_PWM_OUT=y
-px4io,CONFIG_DRIVERS_PX4IO=y
-rc_input,CONFIG_DRIVERS_RC_INPUT=y
-roboclaw,CONFIG_DRIVERS_ROBOCLAW=y
-rpi_rc_in,CONFIG_DRIVERS_RPI_RC_IN=y
-rpm,CONFIG_DRIVERS_RPM=y
-safety_button,CONFIG_DRIVERS_SAFETY_BUTTON=y
-smart_battery/batmon,CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
-spektrum_rc,CONFIG_DRIVERS_SPEKTRUM_RC=y
-telemetry,CONFIG_DRIVERS_TELEMETRY=y
-test_ppm,CONFIG_DRIVERS_TEST_PPM=y
-tone_alarm,CONFIG_DRIVERS_TONE_ALARM=y
-uavcan,CONFIG_DRIVERS_UAVCAN=y
-uavcannode,CONFIG_DRIVERS_UAVCANNODE=y
-uavcannode_gps_demo,CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y
-airship_att_control,CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
-airspeed_selector,CONFIG_MODULES_AIRSPEED_SELECTOR=y
-velocity_controller,CONFIG_MODULES_ANGULAR_VELOCITY_CONTROLLER=y
-attitude_estimator_q,CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
-battery_status,CONFIG_MODULES_BATTERY_STATUS=y
-camera_feedback,CONFIG_MODULES_CAMERA_FEEDBACK=y
-commander,CONFIG_MODULES_COMMANDER=y
-control_allocator,CONFIG_MODULES_CONTROL_ALLOCATOR=y
-dataman,CONFIG_MODULES_DATAMAN=y
-ekf2,CONFIG_MODULES_EKF2=y
-esc_battery,CONFIG_MODULES_ESC_BATTERY=y
-events,CONFIG_MODULES_EVENTS=y
-flight_mode_manager,CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-fw_att_control,CONFIG_MODULES_FW_ATT_CONTROL=y
-fw_pos_control_l1,CONFIG_MODULES_FW_POS_CONTROL_L1=y
-gyro_calibration,CONFIG_MODULES_GYRO_CALIBRATION=y
-gyro_fft,CONFIG_MODULES_GYRO_FFT=y
-land_detector,CONFIG_MODULES_LAND_DETECTOR=y
-landing_target_estimator,CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
-load_mon,CONFIG_MODULES_LOAD_MON=y
-local_position_estimator,CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
-logger,CONFIG_MODULES_LOGGER=y
-mavlink,CONFIG_MODULES_MAVLINK=y
-mc_att_control,CONFIG_MODULES_MC_ATT_CONTROL=y
-mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
-mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
-mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
-micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
-microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y
-navigator,CONFIG_MODULES_NAVIGATOR=y
-px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
-rc_update,CONFIG_MODULES_RC_UPDATE=y
-replay,CONFIG_MODULES_REPLAY=y
-rover_pos_control,CONFIG_MODULES_ROVER_POS_CONTROL=y
-sensors,CONFIG_MODULES_SENSORS=y
-sih,CONFIG_MODULES_SIH=y
-simulator,CONFIG_MODULES_SIMULATOR=y
-temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
-uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
-gimbal,CONFIG_MODULES_GIMBAL=y
-vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
-bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
-dmesg,CONFIG_SYSTEMCMDS_DMESG=y
-dumpfile,CONFIG_SYSTEMCMDS_DUMPFILE=y
-dyn,CONFIG_SYSTEMCMDS_DYN=y
-failure,CONFIG_SYSTEMCMDS_FAILURE=y
-gpio,CONFIG_SYSTEMCMDS_GPIO=y
-hardfault_log,CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
-i2cdetect,CONFIG_SYSTEMCMDS_I2CDETECT=y
-led_control,CONFIG_SYSTEMCMDS_LED_CONTROL=y
-mft,CONFIG_SYSTEMCMDS_MFT=y
-microbench,CONFIG_SYSTEMCMDS_MICROBENCH=y
-mixer,CONFIG_SYSTEMCMDS_MIXER=y
-motor_test,CONFIG_SYSTEMCMDS_MOTOR_TEST=y
-mtd,CONFIG_SYSTEMCMDS_MTD=y
-netman,CONFIG_SYSTEMCMDS_NETMAN=y
-nshterm,CONFIG_SYSTEMCMDS_NSHTERM=y
-param,CONFIG_SYSTEMCMDS_PARAM=y
-perf,CONFIG_SYSTEMCMDS_PERF=y
-pwm,CONFIG_SYSTEMCMDS_PWM=y
-reboot,CONFIG_SYSTEMCMDS_REBOOT=y
-reflect,CONFIG_SYSTEMCMDS_REFLECT=y
-sd_bench,CONFIG_SYSTEMCMDS_SD_BENCH=y
-serial_tet,CONFIG_SYSTEMCMDS_SERIAL_TEST=y
-shutdown,CONFIG_SYSTEMCMDS_SHUTDOWN=y
-system_time,CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-tests,CONFIG_SYSTEMCMDS_TESTS=y
-top,CONFIG_SYSTEMCMDS_TOP=y
-topic_listener,CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-tune_control,CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
-uorb,CONFIG_SYSTEMCMDS_UORB=y
-usb_connected,CONFIG_SYSTEMCMDS_USB_CONNECTED=y
-ver,CONFIG_SYSTEMCMDS_VER=y
-work_queue,CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-dyn_hello,CONFIG_EXAMPLES_DYN_HELLO=y
-fake_gps,CONFIG_EXAMPLES_FAKE_GPS=y
-fake_gyro,CONFIG_EXAMPLES_FAKE_GYRO=y
-fake_imu,CONFIG_EXAMPLES_FAKE_IMU=y
-fake_magnetometer,CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y
-fixedwing_control,CONFIG_EXAMPLES_FIXEDWING_CONTROL=y
-hello,CONFIG_EXAMPLES_HELLO=y
-hwtest,CONFIG_EXAMPLES_HWTEST=y
-matlab_csv_serial,CONFIG_EXAMPLES_MATLAB_CSV_SERIAL=y
-px4_mavlink_debug,CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
-px4_simple_app,CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
-rover_steering_control,CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y
-uuv_example_app,CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y
-work_item,CONFIG_EXAMPLES_WORK_ITEM=y
-add_compile_options(-Wno-narrowing),CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
-D__PX4_LINUX,CONFIG_BOARD_LINUX=y
@@ -16,7 +16,7 @@ class ModuleDocumentation(object):
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']

    max_line_length = 80 # wrap lines that are longer than this

@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
@@ -10,7 +11,16 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
+# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -3,6 +3,8 @@
 # board sensors init
 #------------------------------------------------------------------------------

+param set-default IMU_GYRO_RATEMAX 1000
+
 # Internal SPI
 paw3902 -s start -Y 180

@@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -14,5 +13,18 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -4,6 +4,7 @@
 #------------------------------------------------------------------------------

 param set-default CBRK_IO_SAFETY 0
+param set-default MBE_ENABLE 1

 safety_button start
 tone_alarm start
@@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
-CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -14,5 +13,18 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -5,6 +5,7 @@

 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_GPS_RTCM 1
+param set-default MBE_ENABLE 1

 safety_button start
 tone_alarm start
@@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MFT=y
@@ -141,6 +141,7 @@
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
 #define PWM_MOTOR_OFF	0
+#define PWM_SERVO_STOP	0
 #define PWM_DEFAULT_MIN 20
 #define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
@@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
@@ -142,6 +142,7 @@
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
 #define PWM_MOTOR_OFF	0
+#define PWM_SERVO_STOP	0
 #define PWM_DEFAULT_MIN 20
 #define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
@@ -1,6 +1,8 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA"
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -12,5 +14,16 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -118,8 +118,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)

-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */

 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
@@ -106,3 +106,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
+#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y
@@ -7,7 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
@@ -24,14 +24,13 @@ CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
-CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
+#CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -42,6 +41,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
@@ -50,6 +50,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -65,17 +66,15 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -93,7 +92,6 @@ CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
@@ -102,4 +100,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y
+CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y
@@ -5,7 +5,6 @@ CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_DRIVERS_UAVCAN=n
-CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
@@ -13,3 +12,4 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
@@ -175,8 +175,8 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=300
-CONFIG_UART4_TXBUFSIZE=900
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=600
 CONFIG_USART1_RXBUFSIZE=300
 CONFIG_USART1_RXDMA=y
 CONFIG_USART1_TXBUFSIZE=300
@@ -1,16 +1,22 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_ST=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MTD=y
@@ -19,5 +25,7 @@ CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -12,6 +13,18 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -1,5 +1,6 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
@@ -31,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PWM=y
@@ -15,10 +15,10 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
-CONFIG_DRIVERS_OSD=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
@@ -28,8 +28,8 @@ CONFIG_DRIVERS_RPM=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -60,12 +61,12 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -1,6 +1,8 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -10,13 +12,20 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
 CONFIG_SERIAL_PASSTHRU_UBLOX=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -198,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=600
-CONFIG_UART4_RXDMA=y
-CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART4_RXBUFSIZE=1200
+CONFIG_UART4_RXDMA=n
+CONFIG_UART4_TXBUFSIZE=1200
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_SERIAL_CONSOLE=y
@@ -225,10 +225,10 @@ CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_RXDMA=y
 CONFIG_USART3_TXBUFSIZE=3000
 CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_RXDMA=y
-CONFIG_USART6_TXBUFSIZE=1500
-CONFIG_USART6_TXDMA=y
+CONFIG_USART6_RXBUFSIZE=1200
+CONFIG_USART6_RXDMA=n
+CONFIG_USART6_TXBUFSIZE=1200
+CONFIG_USART6_TXDMA=n
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -32,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
@@ -1,11 +1,9 @@
-CONFIG_BOARD_PROTECTED=y
+# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_OPTICAL_FLOW=n
 CONFIG_COMMON_TELEMETRY=n
-CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
@@ -17,8 +15,8 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL_L1=n
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
@@ -29,11 +27,9 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_SYSTEMCMDS_DMESG=n
 CONFIG_SYSTEMCMDS_DUMPFILE=n
-CONFIG_SYSTEMCMDS_ESC_CALIB=n
 CONFIG_SYSTEMCMDS_GPIO=n
 CONFIG_SYSTEMCMDS_I2CDETECT=n
 CONFIG_SYSTEMCMDS_LED_CONTROL=n
-CONFIG_SYSTEMCMDS_MOTOR_RAMP=n
 CONFIG_SYSTEMCMDS_MOTOR_TEST=n
 CONFIG_SYSTEMCMDS_MTD=n
 CONFIG_SYSTEMCMDS_NSHTERM=n
@@ -43,3 +39,6 @@ CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=n
 CONFIG_SYSTEMCMDS_USB_CONNECTED=n
+CONFIG_BOARD_PROTECTED=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
@@ -4,7 +4,9 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
+CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_BOARD_TESTING=y
@@ -1,4 +1,5 @@
 # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
@@ -21,6 +22,7 @@ CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
@@ -30,5 +32,7 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=n
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n
 CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default"
+CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -1,3 +1,4 @@
+CONFIG_COMMON_BAROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -8,5 +9,7 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
+CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
 CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
@@ -34,7 +34,6 @@ CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
-CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
@@ -41,6 +41,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
@@ -65,24 +65,25 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
-	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
+	estimator_aid_source_1d.msg
+	estimator_aid_source_2d.msg
+	estimator_aid_source_3d.msg
 	estimator_baro_bias.msg
 	estimator_event_flags.msg
+	estimator_gps_status.msg
 	estimator_innovations.msg
-	estimator_optical_flow_vel.msg
 	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
-	estimator_aid_source_1d.msg
-	estimator_aid_source_2d.msg
-	estimator_aid_source_3d.msg
 	event.msg
-	follow_target.msg
 	failure_detector_status.msg
+	follow_target.msg
+	follow_target_estimator.msg
+	follow_target_status.msg
 	generator_status.msg
 	geofence_result.msg
 	gimbal_device_attitude_status.msg
@@ -97,8 +98,8 @@ set(msg_files
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
-	internal_combustion_engine_status.msg
 	input_rc.msg
+	internal_combustion_engine_status.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
 	landing_gear.msg
@@ -121,17 +122,16 @@ set(msg_files
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
-	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
 	ping.msg
-	pps_capture.msg
 	position_controller_landing_status.msg
 	position_controller_status.msg
 	position_setpoint.msg
 	position_setpoint_triplet.msg
 	power_button_state.msg
 	power_monitor.msg
+	pps_capture.msg
 	pwm_input.msg
 	px4io_status.msg
 	radio_status.msg
@@ -146,17 +146,18 @@ set(msg_files
 	sensor_baro.msg
 	sensor_combined.msg
 	sensor_correction.msg
-	sensor_gps.msg
 	sensor_gnss_relative.msg
+	sensor_gps.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg
 	sensor_hygrometer.msg
 	sensor_mag.msg
+	sensor_optical_flow.msg
 	sensor_preflight_mag.msg
 	sensor_selection.msg
-	sensors_status_imu.msg
 	sensors_status.msg
+	sensors_status_imu.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg
@@ -174,8 +175,8 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
-	uwb_grid.msg
 	uwb_distance.msg
+	uwb_grid.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_acceleration.msg
@@ -196,6 +197,8 @@ set(msg_files
 	vehicle_local_position_setpoint.msg
 	vehicle_magnetometer.msg
 	vehicle_odometry.msg
+	vehicle_optical_flow.msg
+	vehicle_optical_flow_vel.msg
 	vehicle_rates_setpoint.msg
 	vehicle_roi.msg
 	vehicle_status.msg
@@ -1,8 +0,0 @@
-uint64 timestamp			# time since system start (microseconds)
-uint64 timestamp_sample			# the timestamp of the raw data (microseconds)
-
-float32[2] vel_body			# velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
-float32[2] vel_ne			# same as vel_body but in local frame (m/s)
-float32[2] flow_uncompensated_integral	# integrated optical flow measurement (rad)
-float32[2] flow_compensated_integral	# integrated optical flow measurement compensated for angular motion (rad)
-float32[3] gyro_rate_integral		# gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
@@ -1,8 +1,11 @@
-uint64 timestamp		# time since system start (microseconds)
-float64 lat			# target position (deg * 1e7)
-float64 lon			# target position (deg * 1e7) 
-float32 alt			# target position
-float32 vy			# target vel in y
-float32 vx			# target vel in x
-float32 vz			# target vel in z 
-uint8 est_cap # target reporting capabilities
+uint64 timestamp  # time since system start (microseconds)
+
+float64 lat       # target position (deg * 1e7)
+float64 lon       # target position (deg * 1e7)
+float32 alt       # target position
+
+float32 vy        # target vel in y
+float32 vx        # target vel in x
+float32 vz        # target vel in z
+
+uint8 est_cap     # target reporting capabilities
@@ -0,0 +1,16 @@
+uint64 timestamp                     # time since system start (microseconds)
+uint64 last_filter_reset_timestamp   # time of last filter reset (microseconds)
+
+bool valid              # True if estimator states are okay to be used
+bool stale              # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate.
+
+float64 lat_est         # Estimated target latitude
+float64 lon_est         # Estimated target longitude
+float32 alt_est         # Estimated target altitude
+
+float32[3] pos_est      # Estimated target NED position (m)
+float32[3] vel_est      # Estimated target NED velocity (m/s)
+float32[3] acc_est      # Estimated target NED acceleration (m^2/s)
+
+uint64 prediction_count
+uint64 fusion_count
@@ -0,0 +1,12 @@
+uint64 timestamp                  # [microseconds] time since system start
+
+float32 tracked_target_course     # [rad] Tracked target course in NED local frame (North is course zero)
+float32 follow_angle              # [rad] Current follow angle setting
+
+float32 orbit_angle_setpoint      # [rad] Current orbit angle setpoint from the smooth trajectory generator
+float32 angular_rate_setpoint     # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle
+
+float32[3] desired_position_raw   # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places
+
+bool in_emergency_ascent          # [bool] True when doing emergency ascent (when distance to ground is below safety altitude)
+float32 gimbal_pitch              # [rad] Gimbal pitch commanded to track target in the center of the frame
@@ -1,29 +0,0 @@
-# Optical flow in XYZ body frame in SI units.
-# http://en.wikipedia.org/wiki/International_System_of_Units
-
-uint64 timestamp		# time since system start (microseconds)
-
-uint8 sensor_id			# id of the sensor emitting the flow value
-float32 pixel_flow_x_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
-float32 pixel_flow_y_integral	# accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
-float32 gyro_x_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_y_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 gyro_z_rate_integral	# accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
-float32 ground_distance_m	# Altitude / distance to ground in meters
-uint32 integration_timespan	# accumulation timespan in microseconds
-uint32 time_since_last_sonar_update	# time since last sonar update in microseconds
-uint16 frame_count_since_last_readout	# number of accumulated frames in timespan
-int16 gyro_temperature	# Temperature * 100 in centi-degrees Celsius
-uint8 quality	# Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
-
-float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
-float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
-float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably
-
-
-uint8 MODE_UNKNOWN        = 0
-uint8 MODE_BRIGHT         = 1
-uint8 MODE_LOWLIGHT       = 2
-uint8 MODE_SUPER_LOWLIGHT = 3
-
-uint8 mode
@@ -0,0 +1,30 @@
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
+bool delta_angle_available
+
+float32 distance_m             # (meters) Distance to the center of the flow field
+bool distance_available
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # quality, 0: bad quality, 255: maximum quality
+
+uint32 error_count
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
+
+uint8 MODE_UNKNOWN        = 0
+uint8 MODE_BRIGHT         = 1
+uint8 MODE_LOWLIGHT       = 2
+uint8 MODE_SUPER_LOWLIGHT = 3
+
+uint8 mode
@@ -0,0 +1,6 @@
+module_name: Sagtech MXS
+serial_config:
+    - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM}
+      port_config_param:
+        name: TRANS_MXS_CFG
+        group: Transponders
@@ -40,7 +40,7 @@ rtps:
    receive: true
  - msg:     offboard_control_mode
    receive: true
-  - msg:     optical_flow
+  - msg:     sensor_optical_flow
    receive: true
  - msg:     position_setpoint
    receive: true
@@ -64,5 +64,7 @@ float32[21] velocity_covariance

 uint8 reset_counter

+uint8 quality
+
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
 # TOPICS estimator_odometry estimator_visual_odometry_aligned
@@ -0,0 +1,21 @@
+# Optical flow in XYZ body frame in SI units.
+
+uint64 timestamp               # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id               # unique device ID for the sensor that does not change between power cycles
+
+float32[2] pixel_flow          # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis
+
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable)
+
+float32 distance_m             # (meters) Distance to the center of the flow field (NAN if unavailable)
+
+uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds
+
+uint8 quality                  # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality
+
+float32 max_flow_rate          # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably
+
+float32 min_ground_distance    # (meters) Minimum distance from ground at which the optical flow sensor operates reliably
+float32 max_ground_distance    # (meters) Maximum distance from ground at which the optical flow sensor operates reliably
@@ -0,0 +1,13 @@
+uint64 timestamp                       # time since system start (microseconds)
+uint64 timestamp_sample                # the timestamp of the raw data (microseconds)
+
+float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
+
+float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad)
+float32[2] flow_compensated_integral   # integrated optical flow measurement compensated for angular motion (rad)
+
+float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
+float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
+
+# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel
@@ -3,8 +3,7 @@
 uint64 timestamp				# time since system start (microseconds)

 bool calibration_enabled
-bool system_sensors_initialized
-bool system_hotplug_timeout		# true if the hotplug sensor search is over
+bool pre_flight_checks_pass		# true if all checks necessary to arm pass
 bool auto_mission_available
 bool angular_velocity_valid
 bool attitude_valid
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -43,28 +43,32 @@
 #include <string>
 #include <sstream>
 #include <vector>
+#include <algorithm>
 #include <stdio.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <unistd.h>

 #include "pxh.h"

 namespace px4_daemon
 {

-Pxh *Pxh::_instance = nullptr;
-
 apps_map_type Pxh::_apps = {};
-
+Pxh *Pxh::_instance = nullptr;

 Pxh::Pxh()
 {
 	_history.try_to_add("commander takeoff"); // for convenience
 	_history.reset_to_end();
-	_instance = this;
 }

 Pxh::~Pxh()
 {
-	_instance = nullptr;
+	if (_local_terminal) {
+		tcsetattr(0, TCSANOW, &_orig_term);
+		_instance = nullptr;
+	}
 }

 int Pxh::process_line(const std::string &line, bool silently_fail)
@@ -133,11 +137,167 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
 	}
 }

+void Pxh::_check_remote_uorb_command(std::string &line)
+{
+
+	if (line.empty()) {
+		return;
+	}
+
+	std::stringstream line_stream(line);
+	std::string word;
+
+	line_stream >> word;
+
+	if (word == "uorb") {
+		line += " -1";  // Run uorb command only once
+	}
+}
+
+void Pxh::run_remote_pxh(int remote_in_fd, int remote_out_fd)
+{
+	std::string mystr;
+	int p1[2], pipe_stdout;
+	int p2[2], pipe_stderr;
+	int backup_stdout_fd = dup(STDOUT_FILENO);
+	int backup_stderr_fd = dup(STDERR_FILENO);
+
+	if (pipe(p1) != 0) {
+		perror("Remote shell pipe creation failed");
+		return;
+	}
+
+	if (pipe(p2) != 0) {
+		perror("Remote shell pipe 2 creation failed");
+		close(p1[0]);
+		close(p1[1]);
+		return;
+	}
+
+	// Create pipe to receive stdout and stderr
+	dup2(p1[1], STDOUT_FILENO);
+	close(p1[1]);
+
+	dup2(p2[1], STDERR_FILENO);
+	close(p2[1]);
+
+	pipe_stdout = p1[0];
+	pipe_stderr = p2[0];
+
+	// Set fds for non-blocking operation
+	fcntl(pipe_stdout, F_SETFL, fcntl(pipe_stdout, F_GETFL) | O_NONBLOCK);
+	fcntl(pipe_stderr, F_SETFL, fcntl(pipe_stderr, F_GETFL) | O_NONBLOCK);
+	fcntl(remote_in_fd, F_SETFL, fcntl(remote_in_fd, F_GETFL) | O_NONBLOCK);
+
+	// Check for input on any pipe (i.e. stdout, stderr, or remote_in_fd
+	// stdout and stderr will be sent to the local terminal and a copy of the data
+	// will be sent over to the mavlink shell through the remote_out_fd.
+	//
+	// Any data from remote_in_fd will be process as shell commands when an '\n' is received
+	while (!_should_exit) {
+
+		struct pollfd fds[3] { {pipe_stderr, POLLIN}, {pipe_stdout, POLLIN}, {remote_in_fd, POLLIN}};
+
+		if (poll(fds, 3, -1) == -1) {
+			perror("Mavlink Shell Poll Error");
+			break;
+		}
+
+		if (fds[0].revents & POLLIN) {
+
+			uint8_t buffer[512];
+			size_t len;
+
+			if ((len = read(pipe_stderr, buffer, sizeof(buffer))) <= 0) {
+				break; //EOF or ERROR
+			}
+
+			// Send all the stderr data to the local terminal as well as the remote shell
+			if (write(backup_stderr_fd, buffer, len) <= 0) {
+				perror("Remote shell write stdout");
+				break;
+			}
+
+			if (write(remote_out_fd, buffer, len) <= 0) {
+				perror("Remote shell write");
+				break;
+			}
+
+			// Process all the stderr data first
+			continue;
+		}
+
+		if (fds[1].revents & POLLIN) {
+
+			uint8_t buffer[512];
+			size_t len;
+
+			if ((len = read(pipe_stdout, buffer, sizeof(buffer))) <= 0) {
+				break; //EOF or ERROR
+			}
+
+			// Send all the stdout data to the local terminal as well as the remote shell
+			if (write(backup_stdout_fd, buffer, len) <= 0) {
+				perror("Remote shell write stdout");
+				break;
+			}
+
+			if (write(remote_out_fd, buffer, len) <= 0) {
+				perror("Remote shell write");
+				break;
+			}
+		}
+
+		if (fds[2].revents & POLLIN) {
+
+			char c;
+
+			if (read(remote_in_fd, &c, 1) <= 0) {
+				break; // EOF or ERROR
+			}
+
+			switch (c) {
+
+			case '\n':	// user hit enter
+				printf("\n");
+				_check_remote_uorb_command(mystr);
+				process_line(mystr, false);
+				// reset string
+				mystr = "";
+
+				_print_prompt();
+
+				break;
+
+			default:	// any other input
+				if (c > 3) {
+					fprintf(stdout, "%c", c);
+					fflush(stdout);
+					mystr += (char)c;
+				}
+
+				break;
+			}
+		}
+	}
+
+	// Restore stdout and stderr
+	dup2(backup_stdout_fd, STDOUT_FILENO);
+	dup2(backup_stderr_fd, STDERR_FILENO);
+	close(backup_stdout_fd);
+	close(backup_stderr_fd);
+
+	close(pipe_stdout);
+	close(pipe_stderr);
+	close(remote_in_fd);
+	close(remote_out_fd);
+}

 void Pxh::run_pxh()
 {
-	_should_exit = false;
-
+	// Only the local_terminal needed for static calls
+	_instance = this;
+	_local_terminal = true;
 	_setup_term();

 	std::string mystr;
@@ -154,7 +314,10 @@ void Pxh::run_pxh()

 		switch (c) {
 		case EOF:
-			_should_exit = true;
+			break;
+
+		case '\t':
+			_tab_completion(mystr);
 			break;

 		case 127:	// backslash
@@ -230,7 +393,6 @@ void Pxh::run_pxh()
 			break;
 		}

-
 		if (update_prompt) {
 			// reprint prompt with mystr
 			mystr.insert(mystr.length() - cursor_position, add_string);
@@ -243,14 +405,11 @@ void Pxh::run_pxh()
 				_move_cursor(cursor_position);
 			}
 		}
-
 	}
 }

 void Pxh::stop()
 {
-	_restore_term();
-
 	if (_instance) {
 		_instance->_should_exit = true;
 	}
@@ -294,4 +453,101 @@ void Pxh::_move_cursor(int position)
 	printf("\033[%dD", position);
 }

+void Pxh::_tab_completion(std::string &mystr)
+{
+	// parse line and get command
+	std::stringstream line(mystr);
+	std::string cmd;
+	line >> cmd;
+
+	// cmd is empty or white space send a list of available commands
+	if (cmd.size() == 0) {
+
+		printf("\n");
+
+		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
+			printf("%s ", it->first.c_str());
+		}
+
+		printf("\n");
+		mystr = "";
+
+	} else {
+
+		// find tab completion matches
+		std::vector<std::string> matches;
+
+		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
+			if (it->first.compare(0, cmd.size(), cmd) == 0) {
+				matches.push_back(it->first);
+			}
+		}
+
+		if (matches.size() >= 1) {
+			// if more than one match print all matches
+			if (matches.size() != 1) {
+				printf("\n");
+
+				for (const auto &item : matches) {
+					printf("%s    ", item.c_str());
+				}
+
+				printf("\n");
+			}
+
+			// find minimum size match
+			size_t min_size = 0;
+
+			for (const auto &item : matches) {
+				if (min_size == 0) {
+					min_size = item.size();
+
+				} else if (item.size() < min_size) {
+					min_size = item.size();
+				}
+			}
+
+			// parse through elements to find longest match
+			std::string longest_match;
+			bool done = false;
+
+			for (int i = 0; i < (int)min_size ; ++i) {
+				bool first_time = true;
+
+				for (const auto &item : matches) {
+					if (first_time) {
+						longest_match += item[i];
+						first_time = false;
+
+					} else if (longest_match[i] != item[i]) {
+						done = true;
+						longest_match.pop_back();
+						break;
+					}
+				}
+
+				if (done) { break; }
+
+				mystr = longest_match;
+			}
+		}
+
+		std::string flags;
+
+		while (line >> cmd) {
+			flags += " " + cmd;
+		}
+
+		// add flags back in when there is a command match
+		if (matches.size() == 1) {
+			if (flags.empty()) {
+				mystr += " ";
+
+			} else {
+				mystr += flags;
+			}
+		}
+	}
+}
+
 } // namespace px4_daemon
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2016-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -72,7 +72,12 @@ public:
 	void run_pxh();

 	/**
-	 * Can be called to stop pxh.
+	 * Run the remote mavlink pxh shell.
+	 */
+	void run_remote_pxh(int remote_in_fd, int remote_out_fd);
+
+	/**
+	 * Can be called to stop all pxh shells.
 	 */
 	static void stop();

@@ -80,11 +85,14 @@ private:
 	void _print_prompt();
 	void _move_cursor(int position);
 	void _clear_line();
+	void _tab_completion(std::string &prefix);
+	void _check_remote_uorb_command(std::string &line);

 	void _setup_term();
 	static void _restore_term();

 	bool _should_exit{false};
+	bool _local_terminal{false};
 	History _history;
 	struct termios _orig_term {};

@@ -150,8 +150,12 @@ Server::_server_main()
 		int n_ready = poll(poll_fds.data(), poll_fds.size(), -1);

 		if (n_ready < 0) {
-			PX4_ERR("poll() failed: %s", strerror(errno));
-			return;
+			// Reboot command causes System Interrupt to stop poll(). This is not an error
+			if (errno != EINTR) {
+				PX4_ERR("poll() failed: %s", strerror(errno));
+			}
+
+			break;
 		}

 		_lock();
@@ -2,8 +2,6 @@ menu "Differential pressure"
    menuconfig COMMON_DIFFERENTIAL_PRESSURE
        bool "Common differential pressure module's"
        default n
-        select DRIVERS_DIFFERENTIAL_PRESSURE_ETS
-        select DRIVERS_DIFFERENTIAL_PRESSURE_MS4515
        select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO
        select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO
        select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X
@@ -86,6 +86,11 @@ struct pwm_output_values {
 */
 #define PWM_MOTOR_OFF	900

+/**
+ * Default value for a servo stop
+ */
+#define PWM_SERVO_STOP	1500
+
 /**
 * Default minimum PWM in us
 */
@@ -68,6 +68,7 @@

 #define DRV_IMU_DEVTYPE_SIM 0x14
 #define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15
+#define DRV_FLOW_DEVTYPE_SIM 0x16

 #define DRV_IMU_DEVTYPE_MPU6000  0x21
 #define DRV_GYR_DEVTYPE_L3GD20   0x22
@@ -136,10 +137,6 @@
 #define DRV_PWM_DEVTYPE_PCA9685     0x69
 #define DRV_ACC_DEVTYPE_BMI088		0x6a
 #define DRV_OSD_DEVTYPE_ATXXXX		0x6b
-#define DRV_FLOW_DEVTYPE_PMW3901	0x6c
-#define DRV_FLOW_DEVTYPE_PAW3902	0x6d
-#define DRV_FLOW_DEVTYPE_PX4FLOW	0x6e
-#define DRV_FLOW_DEVTYPE_PAA3905	0x6f


 #define DRV_DIST_DEVTYPE_LL40LS       0x70
@@ -206,8 +203,16 @@

 #define DRV_GPS_DEVTYPE_SIM 0xAF

+#define DRV_TRNS_DEVTYPE_MXS 0xB0
+
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1

+#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2
+#define DRV_FLOW_DEVTYPE_PMW3901 0xB3
+#define DRV_FLOW_DEVTYPE_PAW3902 0xB4
+#define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5
+#define DRV_FLOW_DEVTYPE_PAA3905 0xB6
+
 #define DRV_DEVTYPE_UNUSED		0xff

 #endif /* _DRV_SENSOR_H */
@@ -43,6 +43,10 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 	I2CSPIDriver(config),
 	_drdy_gpio(config.drdy_gpio)
 {
+	if (_drdy_gpio != 0) {
+		_no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt");
+	}
+
 	float yaw_rotation_degrees = (float)config.custom1;

 	if (yaw_rotation_degrees >= 0.f) {
@@ -52,27 +56,21 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
 		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};

 	} else {
-		// otherwise use the parameter SENS_FLOW_ROT
-		param_t rot = param_find("SENS_FLOW_ROT");
-		int32_t val = 0;
-
-		if (param_get(rot, &val) == PX4_OK) {
-			_rotation = get_rot_matrix((enum Rotation)val);
-
-		} else {
-			_rotation.identity();
-		}
+		_rotation.identity();
 	}
 }

 PAA3905::~PAA3905()
 {
 	// free perf counters
-	perf_free(_sample_perf);
+	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
-	perf_free(_comms_errors);
+	perf_free(_reset_perf);
 	perf_free(_false_motion_perf);
-	perf_free(_register_write_fail_perf);
+	perf_free(_mode_change_bright_perf);
+	perf_free(_mode_change_low_light_perf);
+	perf_free(_mode_change_super_low_light_perf);
+	perf_free(_no_motion_interrupt_perf);
 }

 int PAA3905::init()
@@ -84,35 +82,35 @@ int PAA3905::init()

 	Configure();

-	_previous_collect_timestamp = hrt_absolute_time();
-
 	return PX4_OK;
 }

 int PAA3905::probe()
 {
-	const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+	for (int retry = 0; retry < 3; retry++) {
+		const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+		const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
+		const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);

-	if (Product_ID != PRODUCT_ID) {
-		PX4_ERR("Product_ID: %X", Product_ID);
-		return PX4_ERROR;
+		if (Product_ID != PRODUCT_ID) {
+			PX4_ERR("Product_ID: %X", Product_ID);
+			break;
+		}
+
+		if (Revision_ID != REVISION_ID) {
+			PX4_ERR("Revision_ID: %X", Revision_ID);
+			break;
+		}
+
+		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+			PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
+			break;
+		}
+
+		return PX4_OK;
 	}

-	const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
-
-	if (Revision_ID != REVISION_ID) {
-		PX4_ERR("Revision_ID: %X", Revision_ID);
-		return PX4_ERROR;
-	}
-
-	const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
-
-	if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
-		PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
+	return PX4_ERROR;
 }

 int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
@@ -123,12 +121,14 @@ int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void PAA3905::DataReady()
 {
+	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }

 bool PAA3905::DataReadyInterruptConfigure()
 {
 	if (_drdy_gpio == 0) {
+		_data_ready_interrupt_enabled = false;
 		return false;
 	}

@@ -144,12 +144,12 @@ bool PAA3905::DataReadyInterruptConfigure()

 bool PAA3905::DataReadyInterruptDisable()
 {
+	_data_ready_interrupt_enabled = false;
+
 	if (_drdy_gpio == 0) {
 		return false;
 	}

-	_data_ready_interrupt_enabled = false;
-
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }

@@ -159,8 +159,10 @@ void PAA3905::exit_and_cleanup()
 	I2CSPIDriverBase::exit_and_cleanup();
 }

-void PAA3905::Configure()
+void PAA3905::Reset()
 {
+	perf_count(_reset_perf);
+
 	DataReadyInterruptDisable();
 	ScheduleClear();

@@ -169,274 +171,312 @@ void PAA3905::Configure()
 	px4_usleep(1000);
 	_last_reset = hrt_absolute_time();

-	StandardDetectionSetting();
-	ModeAuto012();
+	_discard_reading = 3;

-	CheckMode();
+	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
+	RegisterRead(0x02);
+	RegisterRead(0x03);
+	RegisterRead(0x04);
+	RegisterRead(0x05);
+	RegisterRead(0x06);
+}

-	switch (_mode) {
-	case Mode::Bright:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
-		break;
+void PAA3905::Configure()
+{
+	Reset();

-	case Mode::LowLight:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
-		break;
+	ConfigureStandardDetectionSetting();

-	case Mode::SuperLowLight:
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
-		break;
-	}
+	ConfigureAutomaticModeSwitching();

 	EnableLed();

-	_discard_reading = 3;
-	_valid_count = 0;
+	// Read Register 0x15. Check Bit [7:6] for AMS mode
+	const uint8_t Observation = RegisterRead(Register::Observation);
+	UpdateMode(Observation);

 	if (DataReadyInterruptConfigure()) {
-		// backup schedule as a watchdog timeout
-		ScheduleDelayed(_scheduled_interval_us * 2);
+		// backup schedule
+		ScheduleDelayed(500_ms);

 	} else {
-		ScheduleOnInterval(_scheduled_interval_us);
+		ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
 	}
 }

-void PAA3905::CheckMode()
+void PAA3905::ConfigureStandardDetectionSetting()
 {
-	// Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable.
-	const uint8_t Observation = RegisterRead(Register::Observation);
+	// Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect
+	// when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is
+	// compromised.

-	// Bit [7:6] AMS mode
-	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x51, 0xFF);
+	RegisterWrite(0x4E, 0x2A);
+	RegisterWrite(0x66, 0x3E);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x7E, 0x71);
+	RegisterWrite(0x55, 0x00);
+	RegisterWrite(0x59, 0x00);
+	RegisterWrite(0x6F, 0x2C);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x4D, 0xAC);
+	RegisterWrite(0x4E, 0x32);
+	RegisterWrite(0x7F, 0x09);
+	RegisterWrite(0x5C, 0xAF);
+	RegisterWrite(0x5F, 0xAF);
+	RegisterWrite(0x70, 0x08);
+	RegisterWrite(0x71, 0x04);
+	RegisterWrite(0x72, 0x06);
+	RegisterWrite(0x74, 0x3C);
+	RegisterWrite(0x75, 0x28);
+	RegisterWrite(0x76, 0x20);
+	RegisterWrite(0x4E, 0xBF);
+	RegisterWrite(0x7F, 0x03);
+	RegisterWrite(0x64, 0x14);
+	RegisterWrite(0x65, 0x0A);
+	RegisterWrite(0x66, 0x10);
+	RegisterWrite(0x55, 0x3C);
+	RegisterWrite(0x56, 0x28);
+	RegisterWrite(0x57, 0x20);
+	RegisterWrite(0x4A, 0x2D);

-	if (ams_mode == 0x0) {
-		// Mode 0
-		_mode = Mode::SuperLowLight;
-
-	} else if (ams_mode == 0x1) {
-		// Mode 1
-		_mode = Mode::LowLight;
-
-	} else if (ams_mode == 0x2) {
-		// Mode 2
-		_mode = Mode::SuperLowLight;
-	}
+	RegisterWrite(0x4B, 0x2D);
+	RegisterWrite(0x4E, 0x4B);
+	RegisterWrite(0x69, 0xFA);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x69, 0x1F);
+	RegisterWrite(0x47, 0x1F);
+	RegisterWrite(0x48, 0x0C);
+	RegisterWrite(0x5A, 0x20);
+	RegisterWrite(0x75, 0x0F);
+	RegisterWrite(0x4A, 0x0F);
+	RegisterWrite(0x42, 0x02);
+	RegisterWrite(0x45, 0x03);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x67, 0x76);
+	RegisterWrite(0x68, 0x76);
+	RegisterWrite(0x6A, 0xC5);
+	RegisterWrite(0x43, 0x00);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x4A, 0x18);
+	RegisterWrite(0x4B, 0x0C);
+	RegisterWrite(0x4C, 0x0C);
+	RegisterWrite(0x4D, 0x0C);
+	RegisterWrite(0x46, 0x0A);
+	RegisterWrite(0x59, 0xCD);
+	RegisterWrite(0x7F, 0x0A);
+	RegisterWrite(0x4A, 0x2A);
+	RegisterWrite(0x48, 0x96);
+	RegisterWrite(0x52, 0xB4);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x5B, 0xA0);
 }

-void PAA3905::StandardDetectionSetting()
+void PAA3905::ConfigureEnhancedDetectionMode()
 {
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x51, 0xFF);
-	RegisterWriteVerified(0x4E, 0x2A);
-	RegisterWriteVerified(0x66, 0x3E);
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x7E, 0x71);
-	RegisterWriteVerified(0x55, 0x00);
-	RegisterWriteVerified(0x59, 0x00);
-	RegisterWriteVerified(0x6F, 0x2C);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x4D, 0xAC);
-	RegisterWriteVerified(0x4E, 0x32);
-	RegisterWriteVerified(0x7F, 0x09);
-	RegisterWriteVerified(0x5C, 0xAF);
-	RegisterWriteVerified(0x5F, 0xAF);
-	RegisterWriteVerified(0x70, 0x08);
-	RegisterWriteVerified(0x71, 0x04);
-	RegisterWriteVerified(0x72, 0x06);
-	RegisterWriteVerified(0x74, 0x3C);
-	RegisterWriteVerified(0x75, 0x28);
-	RegisterWriteVerified(0x76, 0x20);
-	RegisterWriteVerified(0x4E, 0xBF);
-	RegisterWriteVerified(0x7F, 0x03);
-	RegisterWriteVerified(0x64, 0x14);
-	RegisterWriteVerified(0x65, 0x0A);
-	RegisterWriteVerified(0x66, 0x10);
-	RegisterWriteVerified(0x55, 0x3C);
-	RegisterWriteVerified(0x56, 0x28);
-	RegisterWriteVerified(0x57, 0x20);
-	RegisterWriteVerified(0x4A, 0x2D);
+	// Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion
+	// detection is required, and also where more sensitive challenging surface detection is required. The recommended
+	// operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of
+	// sensitivity.

-	RegisterWriteVerified(0x4B, 0x2D);
-	RegisterWriteVerified(0x4E, 0x4B);
-	RegisterWriteVerified(0x69, 0xFA);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x69, 0x1F);
-	RegisterWriteVerified(0x47, 0x1F);
-	RegisterWriteVerified(0x48, 0x0C);
-	RegisterWriteVerified(0x5A, 0x20);
-	RegisterWriteVerified(0x75, 0x0F);
-	RegisterWriteVerified(0x4A, 0x0F);
-	RegisterWriteVerified(0x42, 0x02);
-	RegisterWriteVerified(0x45, 0x03);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x67, 0x76);
-	RegisterWriteVerified(0x68, 0x76);
-	RegisterWriteVerified(0x6A, 0xC5);
-	RegisterWriteVerified(0x43, 0x00);
-	RegisterWriteVerified(0x7F, 0x06);
-	RegisterWriteVerified(0x4A, 0x18);
-	RegisterWriteVerified(0x4B, 0x0C);
-	RegisterWriteVerified(0x4C, 0x0C);
-	RegisterWriteVerified(0x4D, 0x0C);
-	RegisterWriteVerified(0x46, 0x0A);
-	RegisterWriteVerified(0x59, 0xCD);
-	RegisterWriteVerified(0x7F, 0x0A);
-	RegisterWriteVerified(0x4A, 0x2A);
-	RegisterWriteVerified(0x48, 0x96);
-	RegisterWriteVerified(0x52, 0xB4);
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x5B, 0xA0);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x51, 0xFF);
+	RegisterWrite(0x4E, 0x2A);
+	RegisterWrite(0x66, 0x26);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x7E, 0x71);
+	RegisterWrite(0x55, 0x00);
+	RegisterWrite(0x59, 0x00);
+	RegisterWrite(0x6F, 0x2C);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x4D, 0xAC);
+	RegisterWrite(0x4E, 0x65);
+	RegisterWrite(0x7F, 0x09);
+	RegisterWrite(0x5C, 0xAF);
+	RegisterWrite(0x5F, 0xAF);
+	RegisterWrite(0x70, 0x00);
+	RegisterWrite(0x71, 0x00);
+	RegisterWrite(0x72, 0x00);
+	RegisterWrite(0x74, 0x14);
+	RegisterWrite(0x75, 0x14);
+	RegisterWrite(0x76, 0x06);
+	RegisterWrite(0x4E, 0x8F);
+	RegisterWrite(0x7F, 0x03);
+	RegisterWrite(0x64, 0x00);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x66, 0x00);
+	RegisterWrite(0x55, 0x14);
+	RegisterWrite(0x56, 0x14);
+	RegisterWrite(0x57, 0x06);
+	RegisterWrite(0x4A, 0x20);
+
+	RegisterWrite(0x4B, 0x20);
+	RegisterWrite(0x4E, 0x32);
+	RegisterWrite(0x69, 0xFE);
+	RegisterWrite(0x7F, 0x05);
+	RegisterWrite(0x69, 0x14);
+	RegisterWrite(0x47, 0x14);
+	RegisterWrite(0x48, 0x1C);
+	RegisterWrite(0x5A, 0x20);
+	RegisterWrite(0x75, 0xE5);
+	RegisterWrite(0x4A, 0x05);
+	RegisterWrite(0x42, 0x04);
+	RegisterWrite(0x45, 0x03);
+	RegisterWrite(0x65, 0x00);
+	RegisterWrite(0x67, 0x50);
+	RegisterWrite(0x68, 0x50);
+	RegisterWrite(0x6A, 0xC5);
+	RegisterWrite(0x43, 0x00);
+	RegisterWrite(0x7F, 0x06);
+	RegisterWrite(0x4A, 0x1E);
+	RegisterWrite(0x4B, 0x1E);
+	RegisterWrite(0x4C, 0x34);
+	RegisterWrite(0x4D, 0x34);
+	RegisterWrite(0x46, 0x32);
+	RegisterWrite(0x59, 0x0D);
+	RegisterWrite(0x7F, 0x0A);
+	RegisterWrite(0x4A, 0x2A);
+	RegisterWrite(0x48, 0x96);
+	RegisterWrite(0x52, 0xB4);
+	RegisterWrite(0x7F, 0x00);
+	RegisterWrite(0x5B, 0xA0);
 }

-void PAA3905::EnhancedDetectionMode()
-{
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x51, 0xFF);
-	RegisterWriteVerified(0x4E, 0x2A);
-	RegisterWriteVerified(0x66, 0x26);
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x7E, 0x71);
-	RegisterWriteVerified(0x55, 0x00);
-	RegisterWriteVerified(0x59, 0x00);
-	RegisterWriteVerified(0x6F, 0x2C);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x4D, 0xAC);
-	RegisterWriteVerified(0x4E, 0x65);
-	RegisterWriteVerified(0x7F, 0x09);
-	RegisterWriteVerified(0x5C, 0xAF);
-	RegisterWriteVerified(0x5F, 0xAF);
-	RegisterWriteVerified(0x70, 0x00);
-	RegisterWriteVerified(0x71, 0x00);
-	RegisterWriteVerified(0x72, 0x00);
-	RegisterWriteVerified(0x74, 0x14);
-	RegisterWriteVerified(0x75, 0x14);
-	RegisterWriteVerified(0x76, 0x06);
-	RegisterWriteVerified(0x4E, 0x8F);
-	RegisterWriteVerified(0x7F, 0x03);
-	RegisterWriteVerified(0x64, 0x00);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x66, 0x00);
-	RegisterWriteVerified(0x55, 0x14);
-	RegisterWriteVerified(0x56, 0x14);
-	RegisterWriteVerified(0x57, 0x06);
-	RegisterWriteVerified(0x4A, 0x20);
-
-	RegisterWriteVerified(0x4B, 0x20);
-	RegisterWriteVerified(0x4E, 0x32);
-	RegisterWriteVerified(0x69, 0xFE);
-	RegisterWriteVerified(0x7F, 0x05);
-	RegisterWriteVerified(0x69, 0x14);
-	RegisterWriteVerified(0x47, 0x14);
-	RegisterWriteVerified(0x48, 0x1C);
-	RegisterWriteVerified(0x5A, 0x20);
-	RegisterWriteVerified(0x75, 0xE5);
-	RegisterWriteVerified(0x4A, 0x05);
-	RegisterWriteVerified(0x42, 0x04);
-	RegisterWriteVerified(0x45, 0x03);
-	RegisterWriteVerified(0x65, 0x00);
-	RegisterWriteVerified(0x67, 0x50);
-	RegisterWriteVerified(0x68, 0x50);
-	RegisterWriteVerified(0x6A, 0xC5);
-	RegisterWriteVerified(0x43, 0x00);
-	RegisterWriteVerified(0x7F, 0x06);
-	RegisterWriteVerified(0x4A, 0x1E);
-	RegisterWriteVerified(0x4B, 0x1E);
-	RegisterWriteVerified(0x4C, 0x34);
-	RegisterWriteVerified(0x4D, 0x34);
-	RegisterWriteVerified(0x46, 0x32);
-	RegisterWriteVerified(0x59, 0x0D);
-	RegisterWriteVerified(0x7F, 0x0A);
-	RegisterWriteVerified(0x4A, 0x2A);
-	RegisterWriteVerified(0x48, 0x96);
-	RegisterWriteVerified(0x52, 0xB4);
-	RegisterWriteVerified(0x7F, 0x00);
-	RegisterWriteVerified(0x5B, 0xA0);
-}
-
-void PAA3905::ModeAuto012()
+void PAA3905::ConfigureAutomaticModeSwitching()
 {
 	// Automatic switching between Mode 0, 1 and 2:
-	RegisterWriteVerified(0x7F, 0x08);
-	RegisterWriteVerified(0x68, 0x02);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x08);
+	RegisterWrite(0x68, 0x02);
+	RegisterWrite(0x7F, 0x00);
+
+	// TODO: for mode 0 and 1 only
+	// Automatic switching between Mode 0 and 1 only:
+	// RegisterWrite(0x7F, 0x08);
+	// RegisterWrite(0x68, 0x01); // different than mode 0,1,2
+	// RegisterWrite(0x7F, 0x00);
 }

 void PAA3905::EnableLed()
 {
 	// Enable LED_N controls
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x6F, 0x0C);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x6F, 0x0C);
+	RegisterWrite(0x7F, 0x00);
 }

-uint8_t PAA3905::RegisterRead(uint8_t reg, int retries)
+bool PAA3905::UpdateMode(const uint8_t observation)
 {
-	for (int i = 0; i < retries; i++) {
-		px4_udelay(TIME_us_TSRAD);
-		uint8_t cmd[2] {reg, 0};
+	bool mode_changed = false;

-		if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) {
-			return cmd[1];
+	// Bit [7:6] AMS mode
+	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+
+	if (ams_mode == 0x0) {
+		// Mode 0 (Bright)
+		if (_mode != Mode::Bright) {
+			mode_changed = true;
+			perf_count(_mode_change_bright_perf);
 		}
+
+		_mode = Mode::Bright;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
+
+	} else if (ams_mode == 0x1) {
+		// Mode 1 (LowLight)
+		if (_mode != Mode::LowLight) {
+			mode_changed = true;
+			perf_count(_mode_change_low_light_perf);
+		}
+
+		_mode = Mode::LowLight;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
+
+	} else if (ams_mode == 0x2) {
+		// Mode 2 (SuperLowLight)
+		if (_mode != Mode::SuperLowLight) {
+			mode_changed = true;
+			perf_count(_mode_change_super_low_light_perf);
+		}
+
+		_mode = Mode::SuperLowLight;
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
 	}

-	perf_count(_comms_errors);
-	return 0;
+	return mode_changed;
+}
+
+uint8_t PAA3905::RegisterRead(uint8_t reg)
+{
+	// tSWR SPI Time Between Write And Read Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+
+	if (elapsed_last_write < TIME_TSWR_us) {
+		px4_udelay(TIME_TSWR_us - elapsed_last_write);
+	}
+
+	// tSRW/tSRR SPI Time Between Read And Subsequent Commands
+	const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time);
+
+	if (elapsed_last_write < TIME_TSRW_TSRR_us) {
+		px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read);
+	}
+
+	uint8_t cmd[2];
+	cmd[0] = DIR_READ(reg);
+	cmd[1] = 0;
+	transfer(&cmd[0], &cmd[0], sizeof(cmd));
+	hrt_store_absolute_time(&_last_read_time);
+
+	return cmd[1];
 }

 void PAA3905::RegisterWrite(uint8_t reg, uint8_t data)
 {
+	// tSWW SPI Time Between Write Commands
+	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+
+	if (elapsed_last_write < TIME_TSWW_us) {
+		px4_udelay(TIME_TSWW_us - elapsed_last_write);
+	}
+
 	uint8_t cmd[2];
 	cmd[0] = DIR_WRITE(reg);
 	cmd[1] = data;
-
-	if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) {
-		perf_count(_comms_errors);
-	}
-}
-
-bool PAA3905::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries)
-{
-	for (int i = 0; i < retries; i++) {
-		uint8_t cmd[2];
-		cmd[0] = DIR_WRITE(reg);
-		cmd[1] = data;
-		transfer(&cmd[0], nullptr, sizeof(cmd));
-		px4_udelay(TIME_us_TSWW);
-
-		// read back to verify
-		uint8_t data_read = RegisterRead(reg);
-
-		if (data_read == data) {
-			return true;
-		}
-
-		PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read);
-	}
-
-	perf_count(_register_write_fail_perf);
-
-	return false;
+	transfer(&cmd[0], nullptr, sizeof(cmd));
+	hrt_store_absolute_time(&_last_write_time);
 }

 void PAA3905::RunImpl()
 {
-	// backup schedule
-	if (_data_ready_interrupt_enabled) {
-		ScheduleDelayed(_scheduled_interval_us * 2);
-	}
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);

-	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
-	static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s;
+	const hrt_abstime now = hrt_absolute_time();

-	if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) {
+	// force reconfigure if we haven't received valid data for quite some time
+	if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) {
 		Configure();
+		perf_end(_cycle_perf);
 		return;
 	}

-	perf_begin(_sample_perf);
-	perf_count(_interval_perf);
+	hrt_abstime timestamp_sample = now;
+
+	if (_data_ready_interrupt_enabled) {
+		// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+		const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+		if (now < drdy_timestamp_sample + _scheduled_interval_us) {
+			timestamp_sample = drdy_timestamp_sample;
+
+		} else {
+			perf_count(_no_motion_interrupt_perf);
+		}
+
+		// push backup schedule back
+		ScheduleDelayed(500_ms);
+	}

 	struct TransferBuffer {
 		uint8_t cmd = Register::Motion_Burst;
@@ -444,37 +484,52 @@ void PAA3905::RunImpl()
 	} buf{};
 	static_assert(sizeof(buf) == (14 + 1));

-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) {
+		perf_end(_cycle_perf);
 		return;
 	}

-	perf_end(_sample_perf);
-
-	const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp;
-
-	// update for next iteration
-	_previous_collect_timestamp = timestamp_sample;
+	hrt_store_absolute_time(&_last_read_time);

 	if (_discard_reading > 0) {
 		_discard_reading--;
-		ResetAccumulatedData();
-		_valid_count = 0;
+		perf_end(_cycle_perf);
 		return;
 	}

-	CheckMode(); // update _mode variable
+	// Bit [5:0] check if chip is working correctly
+	//  0x3F: chip is working correctly
+	if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) {
+		// Other value: recommend to issue a software reset
+		Configure();
+		perf_end(_cycle_perf);
+		return;
+	}
+
+	if (UpdateMode(buf.data.Observation)) {
+		// update scheduling if mode changed
+		if (!_data_ready_interrupt_enabled) {
+			ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+		}
+	}

 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
 	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x00FF80
 	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x00FF80
 	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
-	const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower;

+	// 23-bit Shutter register
+	const uint8_t Shutter_Lower = buf.data.Shutter_Lower;
+	const uint8_t Shutter_Middle = buf.data.Shutter_Middle;
+	const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
+
+	const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower;
+
+	// Motion since last report and Surface quality non-zero
+	const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred;
+
+	// Number of Features = SQUAL * 4
 	bool data_valid = (buf.data.SQUAL > 0);

 	switch (_mode) {
@@ -513,97 +568,73 @@ void PAA3905::RunImpl()
 	}

 	if (data_valid) {
-		const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
-		const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = timestamp_sample;
+		report.device_id = get_device_id();

-		_flow_dt_sum_usec += dt_flow;
-		_flow_sum_x += delta_x_raw;
-		_flow_sum_y += delta_y_raw;
-		_flow_sample_counter++;
-		_flow_quality_sum += buf.data.SQUAL;
+		report.integration_timespan_us = _scheduled_interval_us;
+		report.quality = buf.data.SQUAL;

-		_valid_count++;
+		// set specs according to datasheet
+		report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+		report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+		report.max_ground_distance = INFINITY; // Datasheet: infinity

-	} else {
-		_valid_count = 0;
-		ResetAccumulatedData();
-		return;
+		switch (_mode) {
+		case Mode::Bright:
+			report.mode = sensor_optical_flow_s::MODE_BRIGHT;
+			break;
+
+		case Mode::LowLight:
+			report.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
+			break;
+
+		case Mode::SuperLowLight:
+			report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
+			break;
+		}
+
+		if (motion_detected) {
+			// only populate flow if data valid (motion and quality > 0)
+			const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
+			const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
+
+			// rotate measurements in yaw from sensor frame to body frame
+			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
+
+			// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
+			static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
+			static constexpr float INCHES_PER_METER = 39.3701f;
+
+			// CPI/m -> radians
+			static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
+
+			report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
+			report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+		}
+
+		report.timestamp = hrt_absolute_time();
+		_sensor_optical_flow_pub.publish(report);
+
+		if (report.quality >= 1) {
+			_last_good_data = report.timestamp_sample;
+		}
 	}

-	// returns if the collect time has not been reached
-	if (_flow_dt_sum_usec < COLLECT_TIME) {
-		return;
-	}
-
-	optical_flow_s report{};
-	report.timestamp = timestamp_sample;
-	//report.device_id = get_device_id();
-
-	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
-	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
-
-	// rotate measurements in yaw from sensor frame to body frame
-	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
-	report.pixel_flow_x_integral = pixel_flow_rotated(0);
-	report.pixel_flow_y_integral = pixel_flow_rotated(1);
-
-	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
-	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
-
-	report.quality = _flow_quality_sum / _flow_sample_counter;
-
-	// No gyro on this board
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
-
-	// set (conservative) specs according to datasheet
-	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
-	report.min_ground_distance = 0.08f; // Datasheet: 80mm
-	report.max_ground_distance = 30.0f; // Datasheet: infinity
-
-
-	switch (_mode) {
-	case Mode::Bright:
-		report.mode = optical_flow_s::MODE_BRIGHT;
-		break;
-
-	case Mode::LowLight:
-		report.mode = optical_flow_s::MODE_LOWLIGHT;
-		break;
-
-	case Mode::SuperLowLight:
-		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
-		break;
-	}
-
-	report.timestamp = hrt_absolute_time();
-	_optical_flow_pub.publish(report);
-
-	if (report.quality > 10) {
-		_last_good_publish = report.timestamp;
-	}
-
-	ResetAccumulatedData();
-}
-
-void PAA3905::ResetAccumulatedData()
-{
-	// reset
-	_flow_dt_sum_usec = 0;
-	_flow_sum_x = 0;
-	_flow_sum_y = 0;
-	_flow_sample_counter = 0;
-	_flow_quality_sum = 0;
+	perf_end(_cycle_perf);
 }

 void PAA3905::print_status()
 {
 	I2CSPIDriverBase::print_status();

-	perf_print_counter(_sample_perf);
+	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-	perf_print_counter(_comms_errors);
+	perf_print_counter(_reset_perf);
 	perf_print_counter(_false_motion_perf);
-	perf_print_counter(_register_write_fail_perf);
+	perf_print_counter(_mode_change_bright_perf);
+	perf_print_counter(_mode_change_low_light_perf);
+	perf_print_counter(_mode_change_super_low_light_perf);
+	perf_print_counter(_no_motion_interrupt_perf);
 }
@@ -32,9 +32,9 @@
 ****************************************************************************/

 /**
- * @file paa3905.hpp
+ * @file PAA3905.hpp
 *
- * Driver for the Pixart paa3905 optical flow sensors connected via SPI.
+ * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip
 */

 #pragma once
@@ -48,16 +48,15 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>

 using namespace time_literals;
 using namespace PixArt_PAA3905;

-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
+#define DIR_WRITE(a) ((a) | Bit7)
+#define DIR_READ(a) ((a) & 0x7F)

 class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
 {
@@ -78,59 +77,61 @@ private:

 	int probe() override;

+	void Reset();
+
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();

-	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
+	uint8_t RegisterRead(uint8_t reg);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
-
-	void EnableLed();
-
-	void StandardDetectionSetting();
-	void EnhancedDetectionMode();
-	void ModeAuto012();
-
-	void CheckMode();

 	void Configure();

-	void ResetAccumulatedData();
+	void ConfigureAutomaticModeSwitching();

-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void ConfigureModeBright();
+	void ConfigureModeLowLight();
+	void ConfigureModeSuperLowLight();

-	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
+	void ConfigureStandardDetectionSetting();
+	void ConfigureEnhancedDetectionMode();

-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	void EnableLed();
+
+	bool UpdateMode(const uint8_t observation);
+
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
+	perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
+	perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	perf_counter_t _no_motion_interrupt_perf{nullptr};

 	const spi_drdy_gpio_t _drdy_gpio;

-	uint64_t _previous_collect_timestamp{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint8_t _flow_sample_counter{0};
-	uint16_t _flow_quality_sum{0};
+	matrix::Dcmf _rotation;

-	matrix::Dcmf	_rotation;
+	int _discard_reading{3};

-	int             _discard_reading{3};
+	Mode _mode{Mode::LowLight};

-	int		_flow_sum_x{0};
-	int		_flow_sum_y{0};
-
-	Mode		_mode{Mode::LowLight};
-
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
-
-	int _valid_count{0};
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};

+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

-	hrt_abstime _last_good_publish{0};
+	hrt_abstime _last_write_time{0};
+	hrt_abstime _last_read_time{0};
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
+
+	hrt_abstime _last_good_data{0};
 	hrt_abstime _last_reset{0};
 };
@@ -33,8 +33,8 @@

 #pragma once

-namespace PixArt_PAA3905
-{
+#include <cstdint>
+
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -45,19 +45,24 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);

-static constexpr uint8_t PRODUCT_ID = 0xA2;
-static constexpr uint8_t REVISION_ID = 0x00;
+namespace PixArt_PAA3905
+{
+
+static constexpr uint8_t PRODUCT_ID         = 0xA2;
+static constexpr uint8_t REVISION_ID        = 0x00;
 static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D;

-static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};  // 50 fps

 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface

-// Various time delay needed for paa3905
-static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
-static constexpr uint32_t TIME_us_TSRAD = 2;
+// Various time delays
+static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
+static constexpr uint32_t TIME_TSWR_us      = 6;  // SPI Time Between Write and Read Commands
+static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And Subsequent Commands (actually 1.5us)
+static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay

 enum Register : uint8_t {
 	Product_ID         = 0x00,
@@ -86,9 +91,20 @@ enum Register : uint8_t {
 	Inverse_Product_ID = 0x5F,
 };

-// Observation
+enum Motion_Bit : uint8_t {
+	MotionOccurred     = Bit7, // Motion since last report
+
+	ChallengingSurface = Bit0, // Challenging surface is detected
+};
+
 enum Observation_Bit : uint8_t {
-	Reset = 0x5A,
+	// Bit [7:6]
+	AMS_mode_0 = 0,
+	AMS_mode_1 = Bit6,
+	AMS_mode_2 = Bit7,
+
+	// Bit [5:0]
+	WorkingCorrectly = 0x3F,
 };

 enum class Mode {
@@ -49,7 +49,7 @@ extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
 	using ThisDriver = PAA3905;
 	BusCLIArguments cli{false, true};
 	cli.custom1 = -1;
-	cli.spi_mode = SPIDEV_MODE0;
+	cli.spi_mode = SPIDEV_MODE3;
 	cli.default_spi_frequency = SPI_SPEED;

 	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * paa3905 Optical Flow
+ * PAA3905 Optical Flow
 *
 * @reboot_required true
 *
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -38,7 +38,7 @@ px4_add_module(
 		paw3902_main.cpp
 		PAW3902.cpp
 		PAW3902.hpp
-		PixArt_PAW3902JF_Registers.hpp
+		PixArt_PAW3902_Registers.hpp
 	DEPENDS
 		conversion
 		drivers__device
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -34,12 +34,12 @@
 /**
 * @file PAW3902.hpp
 *
- * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI.
+ * Driver for the PAW3902JF-TXQT: Optical Motion Tracking Chip
 */

 #pragma once

-#include "PixArt_PAW3902JF_Registers.hpp"
+#include "PixArt_PAW3902_Registers.hpp"

 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -48,16 +48,15 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>

 using namespace time_literals;
-using namespace PixArt_PAW3902JF;
+using namespace PixArt_PAW3902;

-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
+#define DIR_WRITE(a) ((a) | Bit7)
+#define DIR_READ(a) ((a) & 0x7F)

 class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
 {
@@ -78,65 +77,61 @@ private:

 	int probe() override;

+	void Reset();
+
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();

-	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
+	uint8_t RegisterRead(uint8_t reg);
 	void RegisterWrite(uint8_t reg, uint8_t data);
-	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);

-	void EnableLed();
-
-	void ModeBright();
-	void ModeLowLight();
-	void ModeSuperLowLight();
+	void Configure();

 	bool ChangeMode(Mode newMode, bool force = false);

-	void ResetAccumulatedData();
+	void ConfigureModeBright();
+	void ConfigureModeLowLight();
+	void ConfigureModeSuperLowLight();

-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	void EnableLed();

-	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
-	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
-	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
-	perf_counter_t	_mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
-	perf_counter_t	_mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
-	perf_counter_t	_mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};

-	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
+	perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")};
+	perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")};
+	perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")};
+	perf_counter_t _no_motion_interrupt_perf{nullptr};

 	const spi_drdy_gpio_t _drdy_gpio;

-	uint64_t _previous_collect_timestamp{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint8_t _flow_sample_counter{0};
-	uint16_t _flow_quality_sum{0};
+	matrix::Dcmf _rotation;

-	matrix::Dcmf	_rotation;
+	int _discard_reading{3};

-	int             _discard_reading{3};
+	Mode _mode{Mode::LowLight};

-	int		_flow_sum_x{0};
-	int		_flow_sum_y{0};
-
-	Mode		_mode{Mode::LowLight};
-
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0};

 	int _bright_to_low_counter{0};
 	int _low_to_superlow_counter{0};
 	int _low_to_bright_counter{0};
 	int _superlow_to_low_counter{0};

-	int _valid_count{0};
-
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

-	hrt_abstime _last_good_publish{0};
+	hrt_abstime _last_write_time{0};
+	hrt_abstime _last_read_time{0};
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s;
+
+	hrt_abstime _last_good_data{0};
 	hrt_abstime _last_reset{0};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -33,22 +33,36 @@

 #pragma once

-namespace PixArt_PAW3902JF
+#include <cstdint>
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace PixArt_PAW3902
 {

-static constexpr uint8_t PRODUCT_ID = 0x49;
-static constexpr uint8_t REVISION_ID = 0x01;
+static constexpr uint8_t PRODUCT_ID         = 0x49;
+static constexpr uint8_t REVISION_ID        = 0x01;
 static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;

-static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
-static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};  // 50 fps

 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface

-// Various time delay needed for PAW3902
-static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
-static constexpr uint32_t TIME_us_TSRAD = 2;
+// Various time delays
+static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
+static constexpr uint32_t TIME_TSWR_us      = 6;  // SPI Time Between Write and Read Commands
+static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And Subsequent Commands (actually 1.5us)
+static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay

 enum Register : uint8_t {
 	Product_ID         = 0x00,
@@ -75,8 +89,8 @@ enum Register : uint8_t {
 	Inverse_Product_ID = 0x5F,
 };

-enum Product_ID_Bit : uint8_t {
-	Reset = 0x5A,
+enum Motion_Bit : uint8_t {
+	MOT = Bit7, // Motion since last report
 };

 enum class Mode {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,7 +32,7 @@
 ****************************************************************************/

 /**
- * PAW3902 & PAW3903 Optical Flow
+ * PAW3902/PAW3903 Optical Flow
 *
 * @reboot_required true
 *
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -71,13 +71,11 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[])

 	if (!strcmp(verb, "start")) {
 		return ThisDriver::module_start(cli, iterator);
-	}

-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}

-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}

@@ -207,16 +207,6 @@ PMW3901::sensorInit()
 int
 PMW3901::init()
 {
-	// get yaw rotation from sensor frame to body frame
-	param_t rot = param_find("SENS_FLOW_ROT");
-
-	if (rot != PARAM_INVALID) {
-		int32_t val = 0;
-		param_get(rot, &val);
-
-		_yaw_rotation = (enum Rotation)val;
-	}
-
 	/* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
 	SPI::set_lockmode(LOCK_THREADS);

@@ -326,28 +316,24 @@ PMW3901::RunImpl()
 	delta_x = (float)_flow_sum_x / 385.0f;		// proportional factor + convert from pixels to radians
 	delta_y = (float)_flow_sum_y / 385.0f;		// proportional factor + convert from pixels to radians

-	optical_flow_s report{};
-	report.timestamp = timestamp;
+	sensor_optical_flow_s report{};
+	report.timestamp_sample = timestamp;

-	report.pixel_flow_x_integral = static_cast<float>(delta_x);
-	report.pixel_flow_y_integral = static_cast<float>(delta_y);
+	report.pixel_flow[0] = static_cast<float>(delta_x);
+	report.pixel_flow[1] = static_cast<float>(delta_y);

-	// rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
+	// rotate measurements in yaw from sensor frame to body frame
 	float zeroval = 0.0f;
-	rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
+	rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);

-	report.frame_count_since_last_readout = _flow_sample_counter;	// number of frames
-	report.integration_timespan = _flow_dt_sum_usec; 	// microseconds
+	report.integration_timespan_us = _flow_dt_sum_usec; 	// microseconds

-	report.sensor_id = 0;
 	report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;

-
 	/* No gyro on this board */
-	report.gyro_x_rate_integral = NAN;
-	report.gyro_y_rate_integral = NAN;
-	report.gyro_z_rate_integral = NAN;
+	report.delta_angle[0] = NAN;
+	report.delta_angle[1] = NAN;
+	report.delta_angle[2] = NAN;

 	// set (conservative) specs according to datasheet
 	report.max_flow_rate = 5.0f;       // Datasheet: 7.4 rad/s
@@ -360,7 +346,8 @@ PMW3901::RunImpl()
 	_flow_sample_counter = 0;
 	_flow_quality_sum = 0;

-	_optical_flow_pub.publish(report);
+	report.timestamp = hrt_absolute_time();
+	_sensor_optical_flow_pub.publish(report);

 	perf_end(_sample_perf);
 }
@@ -45,10 +45,9 @@
 #include <drivers/device/spi.h>
 #include <conversion/rotation.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>
 #include <px4_platform_common/i2c_spi_buses.h>

 /* Configuration Constants */
@@ -84,14 +83,14 @@ private:

 	const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)

-	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};

 	perf_counter_t _sample_perf;
 	perf_counter_t _comms_errors;

 	uint64_t _previous_collect_timestamp{0};

-	enum Rotation _yaw_rotation;
+	enum Rotation _yaw_rotation {};

 	int _flow_sum_x{0};
 	int _flow_sum_y{0};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -41,8 +41,6 @@

 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <lib/conversion/rotation.h>
-#include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -50,8 +48,7 @@
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/module.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>

 /* Configuration Constants */
 #define I2C_FLOW_ADDRESS_DEFAULT    0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
@@ -89,28 +86,19 @@ public:
 	 * and start a new one.
 	 */
 	void				RunImpl();
-protected:
-	int			probe() override;

 private:
+	int			probe() override;

-	uint8_t _sonar_rotation;
 	bool				_sensor_ok{false};
 	bool				_collect_phase{false};

-	uORB::PublicationMulti<optical_flow_s>		_px4flow_topic{ORB_ID(optical_flow)};
-	uORB::PublicationMulti<distance_sensor_s>	_distance_sensor_topic{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};

 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;

-	enum Rotation       _sensor_rotation;
-	float 				_sensor_min_range{0.0f};
-	float 				_sensor_max_range{0.0f};
-	float 				_sensor_max_flow_rate{0.0f};
-
 	i2c_frame _frame;
-	i2c_integral_frame _frame_integral;

 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -134,15 +122,11 @@ private:

 };

-extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
-
 PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
-	_sonar_rotation(config.rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
-	_sensor_rotation(Rotation::ROTATION_NONE)
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
 }

@@ -166,44 +150,6 @@ PX4FLOW::init()
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;

-	/* get yaw rotation from sensor frame to body frame */
-	param_t rot = param_find("SENS_FLOW_ROT");
-
-	if (rot != PARAM_INVALID) {
-		int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward
-		param_get(rot, &val);
-
-		_sensor_rotation = (enum Rotation)val;
-	}
-
-	/* get operational limits of the sensor */
-	param_t hmin = param_find("SENS_FLOW_MINHGT");
-
-	if (hmin != PARAM_INVALID) {
-		float val = 0.7;
-		param_get(hmin, &val);
-
-		_sensor_min_range = val;
-	}
-
-	param_t hmax = param_find("SENS_FLOW_MAXHGT");
-
-	if (hmax != PARAM_INVALID) {
-		float val = 3.0;
-		param_get(hmax, &val);
-
-		_sensor_max_range = val;
-	}
-
-	param_t ratemax = param_find("SENS_FLOW_MAXR");
-
-	if (ratemax != PARAM_INVALID) {
-		float val = 2.5;
-		param_get(ratemax, &val);
-
-		_sensor_max_flow_rate = val;
-	}
-
 	start();

 	return ret;
@@ -269,6 +215,8 @@ PX4FLOW::collect()
 		return ret;
 	}

+	i2c_integral_frame _frame_integral{};
+
 	if (PX4FLOW_REG == 0) {
 		memcpy(&_frame, val, I2C_FRAME_SIZE);
 		memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
@@ -279,52 +227,37 @@ PX4FLOW::collect()
 	}


-	optical_flow_s report{};
+	DeviceId device_id;
+	device_id.devid = get_device_id();
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
+	device_id.devid_s.address = get_i2c_address();
+
+	sensor_optical_flow_s report{};
+
+	report.timestamp_sample = hrt_absolute_time();
+	report.device_id = device_id.devid;
+
+	report.pixel_flow[0] = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians
+	report.pixel_flow[1] = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians
+
+	report.delta_angle_available = true;
+	report.delta_angle[0] = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[1] = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians
+	report.delta_angle[2] = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians
+
+	report.distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.f; // convert to meters
+	report.distance_available = true;
+
+	report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds
+
+	report.quality = _frame_integral.qual; // 0:bad ; 255 max quality
+
+	report.max_flow_rate = 2.5f;
+	report.min_ground_distance = PX4FLOW_MIN_DISTANCE;
+	report.max_ground_distance = PX4FLOW_MAX_DISTANCE;

 	report.timestamp = hrt_absolute_time();
-	report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
-	report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
-	report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
-	report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
-	report.quality = _frame_integral.qual; //0:bad ; 255 max quality
-	report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
-	report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
-	report.integration_timespan = _frame_integral.integration_timespan; //microseconds
-	report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
-	report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
-	report.sensor_id = 0;
-	report.max_flow_rate = _sensor_max_flow_rate;
-	report.min_ground_distance = _sensor_min_range;
-	report.max_ground_distance = _sensor_max_range;
-
-	/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
-	float zeroval = 0.0f;
-
-	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
-	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
-
-	_px4flow_topic.publish(report);
-
-	/* publish to the distance_sensor topic as well */
-	if (_distance_sensor_topic.get_instance() == 0) {
-		distance_sensor_s distance_report{};
-		DeviceId device_id;
-		device_id.devid = get_device_id();
-		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
-
-		distance_report.timestamp = report.timestamp;
-		distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
-		distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
-		distance_report.current_distance = report.ground_distance_m;
-		distance_report.variance = 0.0f;
-		distance_report.signal_quality = -1;
-		distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-		distance_report.device_id = device_id.devid;
-		distance_report.orientation = _sonar_rotation;
-
-		_distance_sensor_topic.publish(distance_report);
-	}
+	_sensor_optical_flow_pub.publish(report);

 	perf_end(_sample_perf);

@@ -374,28 +307,16 @@ PX4FLOW::print_usage()
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
 	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 35, "Rotation (default=downwards)", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

-int
-px4flow_main(int argc, char *argv[])
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[])
 {
-	int ch;
 	using ThisDriver = PX4FLOW;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED;
-	cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT;

-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
 	const char *verb = cli.optArg();

 	if (!verb) {
@@ -415,13 +336,11 @@ px4flow_main(int argc, char *argv[])
 		}

 		return ThisDriver::module_start(cli, iterator);
-	}

-	if (!strcmp(verb, "stop")) {
+	} else if (!strcmp(verb, "stop")) {
 		return ThisDriver::module_stop(iterator);
-	}

-	if (!strcmp(verb, "status")) {
+	} else if (!strcmp(verb, "status")) {
 		return ThisDriver::module_status(iterator);
 	}

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -50,10 +50,10 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <perf/perf_counter.h>
-#include <systemlib/err.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/uORB.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_optical_flow.h>

 #include "thoneflow_parser.h"

@@ -71,21 +71,20 @@ public:
 	void print_info();

 private:
-	char                     _port[20];
-	Rotation            	 _rotation;
-	int                      _cycle_interval;
-	int                      _fd;
-	char                     _linebuf[5];
-	unsigned                 _linebuf_index;
-	THONEFLOW_PARSE_STATE    _parse_state;
+	char                     _port[20] {};
+	Rotation            	 _rotation{ROTATION_NONE};
+	int                      _cycle_interval{10526};
+	int                      _fd{-1};
+	char                     _linebuf[5] {};
+	unsigned                 _linebuf_index{0};
+	THONEFLOW_PARSE_STATE    _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC};

-	hrt_abstime              _last_read;
+	hrt_abstime              _last_read{0};

-	optical_flow_s           _report;
-	orb_advert_t             _optical_flow_pub;
+	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};

-	perf_counter_t           _sample_perf;
-	perf_counter_t           _comms_errors;
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};

 	/**
 	* Initialise the automatic measurement state machine and start it.
@@ -106,23 +105,8 @@ private:

 };

-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]);
-
 Thoneflow::Thoneflow(const char *port) :
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_rotation(Rotation(0)),
-	_cycle_interval(10526),
-	_fd(-1),
-	_linebuf_index(0),
-	_parse_state(THONEFLOW_PARSE_STATE0_UNSYNC),
-	_last_read(0),
-	_report(),
-	_optical_flow_pub(nullptr),
-	_sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err"))
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
 {
 	/* store port name */
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -206,41 +190,6 @@ Thoneflow::init()
 			ret = PX4_ERROR;
 			break;
 		}
-
-		/* get yaw rotation from sensor frame to body frame */
-		param_t rot = param_find("SENS_FLOW_ROT");
-
-		if (rot != PARAM_INVALID) {
-			int32_t val = 0;
-			param_get(rot, &val);
-
-			_rotation = Rotation(val);
-		}
-
-		/* Initialise report structure */
-		/* No gyro on this board */
-		_report.gyro_x_rate_integral = NAN;
-		_report.gyro_y_rate_integral = NAN;
-		_report.gyro_z_rate_integral = NAN;
-
-		/* Conservative specs according to datasheet */
-		_report.max_flow_rate = 5.0f;           // Datasheet: 7.4 rad/s
-		_report.min_ground_distance = 0.1f;     // Datasheet: 80mm
-		_report.max_ground_distance = 30.0f;    // Datasheet: infinity
-
-		/* Integrated flow is sent at 66fps */
-		_report.frame_count_since_last_readout = 1;
-		_report.integration_timespan = 10526;	// microseconds
-
-		/* Get a publish handle on the optical flow topic */
-		_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report);
-
-		if (_optical_flow_pub == nullptr) {
-			PX4_ERR("Failed to create optical_flow object");
-			ret = PX4_ERROR;
-			break;
-		}
-
 	} while (0);

 	/* Close the fd */
@@ -299,20 +248,33 @@ Thoneflow::collect()

 		_last_read = hrt_absolute_time();

+		// publish sensor_optical_flow
+		sensor_optical_flow_s report{};
+		report.timestamp_sample = hrt_absolute_time();
+
 		/* Parse each byte of read buffer */
 		for (int i = 0; i < ret; i++) {
-			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report);
+			valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report);
 		}

 		/* Publish most recent valid measurement */
 		if (valid) {
-			_report.timestamp = hrt_absolute_time();
+
+			report.device_id = 0; // TODO get_device_id();
+			report.integration_timespan_us = 10526; // microseconds

 			/* Rotate measurements from sensor frame to body frame */
 			float zeroval = 0.0f;
-			rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval);
+			rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);

-			orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report);
+			// Conservative specs according to datasheet
+			report.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
+			report.min_ground_distance = 0.08f;    // Datasheet: 80mm
+			report.max_ground_distance = INFINITY; // Datasheet: infinity
+
+			report.timestamp = hrt_absolute_time();
+
+			_sensor_optical_flow_pub.publish(report);
 		}

 		/* Bytes left to parse */
@@ -478,16 +440,15 @@ $ thoneflow stop

    PRINT_MODULE_USAGE_NAME("thoneflow", "driver");
    PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
    PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-    PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-    PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information");
 }

 } // namespace

-int
-thoneflow_main(int argc, char *argv[])
+extern "C" __EXPORT int thoneflow_main(int argc, char *argv[])
 {
    int ch;
    const char *device_path = "";
@@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[])
            thoneflow::usage();
            return -1;
        }
-    }

-    /*
-     * Stop the driver
-     */
-    if (!strcmp(argv[myoptind], "stop")) {
+    } else if (!strcmp(argv[myoptind], "stop")) {
        return thoneflow::stop();
-    }

-    /*
-     * Print driver information.
-     */
-    if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+    } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
        thoneflow::info();
        return 0;
    }
@@ -61,7 +61,7 @@ const char *parser_state[] = {
 #endif

 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *flow)
+		     sensor_optical_flow_s *flow)
 {
 	bool parsed_packet = false;

@@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH
 				// Checksum valid, populate sensor report
 				int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0];
 				int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2];
-				flow->pixel_flow_x_integral = static_cast<float>(delta_x) * (3.52e-3f);
-				flow->pixel_flow_y_integral = static_cast<float>(delta_y) * (3.52e-3f);
+				flow->pixel_flow[0] = static_cast<float>(delta_x) * (3.52e-3f);
+				flow->pixel_flow[1] = static_cast<float>(delta_y) * (3.52e-3f);
 				*state = THONEFLOW_PARSE_STATE7_CHECKSUM;

 			} else {
@@ -33,7 +33,7 @@

 #pragma once

-#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/sensor_optical_flow.h>

 // Data Format for ThoneFlow 3901U
 // ===============================
@@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE {
 };

 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     optical_flow_s *report);
+		     sensor_optical_flow_s *report);
@@ -613,6 +613,14 @@ void PWMOut::update_params()
 						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
 						param_set(param_find(str), &pwm_fail_new);
 					}
+
+				} else {
+					if (pwm_default_channel_mask & 1 << i) {
+						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
+
+					} else {
+						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
+					}
 				}

 			} else {
@@ -803,6 +803,14 @@ void PX4IO::update_params()
 						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
 						param_set(param_find(str), &pwm_fail_new);
 					}
+
+				} else {
+					if (pwm_default_channel_mask & 1 << i) {
+						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
+
+					} else {
+						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
+					}
 				}
 			}
 		}
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,7 +31,4 @@
 #
 ############################################################################

-add_subdirectory(paw3902)
-add_subdirectory(pmw3901)
-add_subdirectory(px4flow)
-add_subdirectory(thoneflow)
+add_subdirectory(sagetech_mxs)
@@ -0,0 +1,10 @@
+menu "Transponder"
+    menuconfig TRANSPONDER
+        bool "Transponder"
+        default n
+        select DRIVERS_TRANSPONDER_SAGETECH_MXS
+        ---help---
+            Enable default set of transponder drivers
+
+    rsource "*/Kconfig"
+endmenu #transponder
@@ -0,0 +1,82 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__transponder__sagetech_mxs
+	MAIN sagetech_mxs
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		sg_sdk/appendChecksum.c
+		sg_sdk/float2Buf.c
+		sg_sdk/sgDecodeFlightId.c
+		sg_sdk/sgDecodeSVR.c
+		sg_sdk/sgEncodeGPS.c
+		sg_sdk/sgEncodeTargetReq.c
+		sg_sdk/target.c
+		sg_sdk/toGS.c
+		sg_sdk/toIcao.c
+		sg_sdk/toLatLon.c
+		sg_sdk/toUint32.c
+		sg_sdk/uint322Buf.c
+		sg_sdk/calcChecksum.c
+		sg_sdk/icao2Buf.c
+		sg_sdk/sgDecodeInstall.c
+		sg_sdk/sgEncodeDataReq.c
+		sg_sdk/sgEncodeInstall.c
+		sg_sdk/toHeading2.c
+		sg_sdk/toInt16.c
+		sg_sdk/toTOA.c
+		sg_sdk/toVel.c
+		sg_sdk/charArray2Buf.c
+		sg_sdk/sgDecodeAck.c
+		sg_sdk/sgDecodeMSR.c
+		sg_sdk/sgEncodeFlightId.c
+		sg_sdk/sgEncodeOperating.c
+		sg_sdk/toAlt.c
+		sg_sdk/toHeading.c
+		sg_sdk/toInt32.c
+		sg_sdk/toUint16.c
+		sg_sdk/uint162Buf.c
+		sg_sdk/sagetech_mxs.h
+		sg_sdk/sg.h
+		sg_sdk/target.h
+		sg_sdk/sgUtil.h
+		SagetechMXS.cpp
+		SagetechMXS.hpp
+	DEPENDS
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_TRANSPONDER_SAGETECH_MXS
+	bool "sagetech_mxs"
+	default n
+	---help---
+		Enable support for my_work_item
@@ -0,0 +1,279 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/cli.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
+
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <drivers/device/device.h>
+#include <lib/geo/geo.h>
+#include <termios.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/transponder_report.h>
+#include <uORB/topics/vehicle_land_detected.h>
+
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "sg_sdk/sagetech_mxs.h"
+
+using namespace time_literals;
+
+class SagetechMXS : public ModuleBase<SagetechMXS>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	SagetechMXS(const char *port);
+	~SagetechMXS() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	void print_info();
+
+	bool init();
+
+	int print_status() override;
+
+private:
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::ADSB_SQUAWK>)		_adsb_squawk,
+		(ParamInt<px4::params::ADSB_IDENT>)		_adsb_ident,
+		(ParamInt<px4::params::ADSB_LIST_MAX>)		_adsb_list_max,
+		(ParamInt<px4::params::ADSB_ICAO_ID>)		_adsb_icao,
+		(ParamInt<px4::params::ADSB_LEN_WIDTH>)		_adsb_len_width,
+		(ParamInt<px4::params::ADSB_EMIT_TYPE>)		_adsb_emit_type,
+		(ParamInt<px4::params::ADSB_MAX_SPEED>)		_adsb_max_speed,
+		(ParamInt<px4::params::ADSB_ICAO_SPECL>)	_adsb_icao_specl,
+		(ParamInt<px4::params::ADSB_EMERGC>)		_adsb_emergc,
+		(ParamInt<px4::params::MXS_OP_MODE>)		_mxs_op_mode,
+		(ParamInt<px4::params::MXS_TARG_PORT>)		_mxs_targ_port,
+		// (ParamInt<px4::params::MXS_COM0_BAUD>)		_mxs_com0_baud,
+		// (ParamInt<px4::params::MXS_COM1_BAUD>)		_mxs_com1_baud,
+		(ParamInt<px4::params::MXS_EXT_CFG>)		_mxs_ext_cfg,
+		(ParamInt<px4::params::SER_MXS_BAUD>)		_ser_mxs_baud
+	);
+
+	// Serial Port Variables
+	char _port[20] {};
+	int  _fd{-1};
+
+	// Publications
+	uORB::Publication<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+
+	// Subscriptions
+	uORB::Subscription                 _sensor_gps_sub{ORB_ID(sensor_gps)};
+	uORB::SubscriptionInterval         _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
+	uORB::Subscription                 _vehicle_status_sub{ORB_ID(vehicle_status)};          // regular subscription for additional data
+	uORB::Subscription                 _transponder_report_sub{ORB_ID(transponder_report)};
+	uORB::Subscription                 _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+
+
+	// Performance (perf) counters
+	perf_counter_t	_loop_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": run_loop")};
+	perf_counter_t	_loop_elapsed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t  _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t  _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
+
+	// Constants
+	static constexpr uint32_t UPDATE_INTERVAL_US{1000000 / 50}; 	// 20ms = 50 Hz
+	static constexpr uint8_t FIVE_HZ_MOD{10};			// 0.2s = 5 Hz
+	static constexpr uint8_t TWO_HZ_MOD{25};			// 0.5s = 2 Hz
+	static constexpr uint8_t ONE_HZ_MOD{50};			// 1 Hz
+	static constexpr uint16_t EIGHT_TWO_SEC_MOD{410};		// 8.2 seconds
+	// static constexpr uint8_t  SG_MSG_START_BYTE{0xAA};
+	static constexpr uint32_t PAYLOAD_MXS_MAX_SIZE{255};
+	static constexpr float SAGETECH_SCALE_FEET_TO_M{0.3048F};
+	static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F};
+	static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F};
+	static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F};
+	static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F};
+	static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F};
+	static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0};
+	static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1};
+	static constexpr uint16_t MAX_VEHICLES_LIMIT{50};
+	static constexpr float SAGETECH_HPL_UNKNOWN{38000.0F};
+	static constexpr float CLIMB_RATE_LIMIT{16448};
+	static constexpr uint16_t MXS_INIT_TIMEOUT_COUNT{1000};		// 1000 loop cycles = 20 seconds
+	static constexpr uint8_t BASE_OCTAL{8};
+	static constexpr uint8_t BASE_HEX{16};
+	static constexpr uint8_t BASE_DEC{10};
+	static constexpr uint16_t INVALID_SQUAWK{7777};
+	static constexpr unsigned BAUD_460800{0010004};			// B460800 not defined in MacOS termios
+	static constexpr unsigned BAUD_921600{0010007};			// B921600 not defined in MacOS termios
+
+	// Stored variables
+	uint64_t _loop_count;
+
+	// Cached Subscription Data
+	sensor_gps_s _gps;
+	vehicle_land_detected_s _landed;
+
+	enum class MsgType : uint8_t {
+		Installation            = SG_MSG_TYPE_HOST_INSTALL,
+		FlightID                = SG_MSG_TYPE_HOST_FLIGHT,
+		Operating               = SG_MSG_TYPE_HOST_OPMSG,
+		GPS_Data                = SG_MSG_TYPE_HOST_GPS,
+		Data_Request            = SG_MSG_TYPE_HOST_DATAREQ,
+		// RESERVED 0x06 - 0x0A
+		Target_Request          = SG_MSG_TYPE_HOST_TARGETREQ,
+		Mode                    = SG_MSG_TYPE_HOST_MODE,
+		// RESERVED 0x0D - 0xC1
+		ACK                     = SG_MSG_TYPE_XPNDR_ACK,
+		Installation_Response   = SG_MSG_TYPE_XPNDR_INSTALL,
+		FlightID_Response       = SG_MSG_TYPE_XPNDR_FLIGHT,
+		Status_Response         = SG_MSG_TYPE_XPNDR_STATUS,
+		RESERVED_0x84           = 0x84,
+		RESERVED_0x85           = 0x85,
+		Mode_Settings           = SG_MSG_TYPE_XPNDR_MODE,
+		RESERVED_0x8D           = 0x8D,
+		Version_Response        = SG_MSG_TYPE_XPNDR_VERSION,
+		Serial_Number_Response  = SG_MSG_TYPE_XPNDR_SERIALNUM,
+		Target_Summary_Report   = SG_MSG_TYPE_ADSB_TSUMMARY,
+
+		ADSB_StateVector_Report = SG_MSG_TYPE_ADSB_SVR,
+		ADSB_ModeStatus_Report  = SG_MSG_TYPE_ADSB_MSR,
+		ADSB_Target_State_Report = SG_MSG_TYPE_ADSB_TSTATE,
+		ADSB_Air_Ref_Vel_Report = SG_MSG_TYPE_ADSB_ARVR,
+	};
+
+	enum class ParseState {
+		WaitingFor_Start,
+		WaitingFor_MsgType,
+		WaitingFor_MsgId,
+		WaitingFor_PayloadLen,
+		WaitingFor_PayloadContents,
+		WaitingFor_Checksum,
+	};
+
+	struct __attribute__((packed)) Packet {
+		const uint8_t   start = SG_MSG_START_BYTE;
+		MsgType         type;
+		uint8_t         id;
+		uint8_t         payload_length;
+		uint8_t         payload[PAYLOAD_MXS_MAX_SIZE];
+	};
+
+	struct {
+		ParseState      state;
+		uint8_t         index;
+		Packet          packet;
+		uint8_t         checksum;
+	} _message_in;
+
+	struct {
+		bool initialized;
+		bool init_failed;
+		sg_operating_t op;
+		sg_install_t inst;
+		// sg_gps_t gps;
+		sg_targetreq_t treq;
+		sg_flightid_t fid;
+		sg_ack_t ack;
+	} mxs_state;
+
+	struct {
+		// cached variables to compare against params so we can send msg on param change.
+		bool            failXpdr;
+		bool            failSystem;
+		struct {
+			uint8_t     id;
+			uint8_t     type;
+		} msg;
+	} last;
+
+	// External Vehicle List
+	transponder_report_s *vehicle_list;
+	uint16_t list_size_allocated;
+	uint16_t vehicle_count = 0;
+	uint16_t furthest_vehicle_index = 0;
+	float furthest_vehicle_distance = 0;
+
+	// Functions
+	void Run() override;
+	void handle_packet(const Packet &msg);
+	int msg_write(const uint8_t *data, const uint16_t len) const;
+	bool parse_byte(const uint8_t data);
+	void handle_ack(const sg_ack_t ack);
+	void handle_svr(const sg_svr_t svr);
+	void handle_msr(sg_msr_t msr);
+	bool get_vehicle_by_ICAO(const uint32_t icao, transponder_report_s &vehicle) const;
+	bool find_index(const transponder_report_s &vehicle, uint16_t *index) const;
+	void set_vehicle(const uint16_t index, const transponder_report_s &vehicle);
+	void delete_vehicle(const uint16_t index);
+	bool is_special_vehicle(uint32_t icao) const {return _adsb_icao_specl.get() != 0 && (_adsb_icao_specl.get() == (int32_t) icao);}
+	void handle_vehicle(const transponder_report_s &vehicle);
+	void determine_furthest_aircraft();
+	void send_data_req(const sg_datatype_t dataReqType);
+	void send_install_msg();
+	void send_flight_id_msg();
+	void send_operating_msg();
+	void send_gps_msg();
+	void send_targetreq_msg();
+	uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber);
+	int open_serial_port();
+	sg_emitter_t convert_emitter_type_to_sg(int emitType);
+	int convert_sg_to_emitter_type(sg_emitter_t sg_emit);
+	int handle_fid(const char *fid);
+	int store_inst_resp();
+	void auto_config_operating();
+	void auto_config_installation();
+	void auto_config_flightid();
+	unsigned convert_to_px4_baud(int baudType);
+	bool check_valid_squawk(int squawk);
+};
@@ -0,0 +1,8 @@
+module_name: Sagetech MXS
+serial_config:
+    - command: sagetech_mxs start -d ${SERIAL_DEV}
+      port_config_param:
+        name: MXS_SER_CFG
+        group: Transponder
+        default: TEL2
+      label: Sagetech MXS Serial Port
@@ -0,0 +1,269 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * parameters.c
+ *
+ * Sagetech MXS transponder custom parameters
+ * @author Megan McCormick megan.mccormick@sagetech.com
+ * @author Check Faber chuck.faber@sagetech.com
+ */
+
+/**
+ * ADSB-Out squawk code configuration
+ *
+ * This parameter defines the squawk code. Value should be between 0000 and 7777.
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 7777
+ * @group Transponder
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_SQUAWK, 1200);
+
+/**
+ * ADSB-Out Ident Configuration
+ *
+ * Enable Identification of Position feature
+ *
+ * @boolean
+ * @reboot_required false
+ * @group Transponder
+ */
+PARAM_DEFINE_INT32(ADSB_IDENT, 0);
+
+/**
+ * ADSB-In Vehicle List Size
+ *
+ * Change number of targets to track
+ *
+ * @min 0
+ * @max 50
+ * @reboot_required true
+ * @group Transponder
+ */
+PARAM_DEFINE_INT32(ADSB_LIST_MAX, 25);
+
+/**
+ * ADSB-Out ICAO configuration
+ *
+ * Defines the ICAO ID of the vehicle
+ *
+ * @reboot_required true
+ * @min -1
+ * @max 16777215
+ * @group Transponder
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_ICAO_ID, 1194684);
+
+/**
+ * ADSB-Out Vehicle Size Configuration
+ *
+ * Report the length and width of the vehicle in meters. In most cases, use '1' for the smallest vehicle size.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 15
+ * @group Transponder
+ *
+ * @value 0 SizeUnknown
+ * @value 1 Len15_Wid23
+ * @value 2 Len25_Wid28
+ * @value 3 Len25_Wid34
+ * @value 4 Len35_Wid33
+ * @value 5 Len35_Wid38
+ * @value 6 Len45_Wid39
+ * @value 7 Len45_Wid45
+ * @value 8 Len55_Wid45
+ * @value 9 Len55_Wid52
+ * @value 10 Len65_Wid59
+ * @value 11 Len65_Wid67
+ * @value 12 Len75_Wid72
+ * @value 13 Len75_Wid80
+ * @value 14 Len85_Wid80
+ * @value 15 Len85_Wid90
+ */
+PARAM_DEFINE_INT32(ADSB_LEN_WIDTH, 1);
+
+/**
+ * ADSB-Out Vehicle Emitter Type
+ *
+ * Configure the emitter type of the vehicle.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 15
+ * @group Transponder
+ *
+ * @value 0 Unknown
+ * @value 1 Light
+ * @value 2 Small
+ * @value 3 Large
+ * @value 4 HighVortex
+ * @value 5 Heavy
+ * @value 6 Performance
+ * @value 7 Rotorcraft
+ * @value 8 RESERVED
+ * @value 9 Glider
+ * @value 10 LightAir
+ * @value 11 Parachute
+ * @value 12 UltraLight
+ * @value 13 RESERVED
+ * @value 14 UAV
+ * @value 15 Space
+ * @value 16 RESERVED
+ * @value 17 EmergencySurf
+ * @value 18 ServiceSurf
+ * @value 19 PointObstacle
+ */
+PARAM_DEFINE_INT32(ADSB_EMIT_TYPE, 14);
+
+/**
+ * ADSB-Out Vehicle Max Speed
+ *
+ * Informs ADSB vehicles of this vehicle's max speed capability
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 6
+ * @value 0 UnknownMaxSpeed
+ * @value 1 75Kts
+ * @value 2 150Kts
+ * @value 3 300Kts
+ * @value 4 600Kts
+ * @value 5 1200Kts
+ * @value 6 Over1200Kts
+ * @group Transponder
+ */
+PARAM_DEFINE_INT32(ADSB_MAX_SPEED, 0);
+
+/**
+ * ADSB-In Special ICAO configuration
+ *
+ * This vehicle is always tracked. Use 0 to disable.
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 16777215
+ * @group Transponder
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0);
+
+/**
+ * ADSB-Out Emergency State
+ *
+ * Sets the vehicle emergency state
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 6
+ * @value 0 NoEmergency
+ * @value 1 General
+ * @value 2 Medical
+ * @value 3 LowFuel
+ * @value 4 NoCommunications
+ * @value 5 Interference
+ * @value 6 Downed
+ * @group Transponder
+ */
+PARAM_DEFINE_INT32(ADSB_EMERGC, 0);
+
+/**
+ * Sagetech MXS mode configuration
+ *
+ * This parameter defines the operating mode of the MXS
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 3
+ * @group Transponder
+ *
+ * @value 0 Off
+ * @value 1 On
+ * @value 2 Standby
+ * @value 3 Alt
+ */
+PARAM_DEFINE_INT32(MXS_OP_MODE, 0);
+
+/**
+ * Sagetech MXS Participant Configuration
+ *
+ * The MXS communication port to receive Target data from
+ *
+ * @min 0
+ * @max 2
+ * @reboot_required false
+ * @group Transponder
+ *
+ * @value 0 Auto
+ * @value 1 COM0
+ * @value 2 COM1
+ */
+PARAM_DEFINE_INT32(MXS_TARG_PORT, 1);
+
+/**
+ * Sagetech External Configuration Mode
+ *
+ * Disables auto-configuration mode enabling MXS config through external software.
+ *
+ * @reboot_required true
+ * @boolean
+ * @group Transponder
+ */
+PARAM_DEFINE_INT32(MXS_EXT_CFG, 0);
+
+/**
+ * MXS Serial Communication Baud rate
+ *
+ * Baudrate for the Serial Port connected to the MXS Transponder
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 10
+ * @value 0 38400
+ * @value 1 600
+ * @value 2 4800
+ * @value 3 9600
+ * @value 4 RESERVED
+ * @value 5 57600
+ * @value 6 115200
+ * @value 7 230400
+ * @value 8 19200
+ * @value 9 460800
+ * @value 10 921600
+ * @group Serial
+ */
+PARAM_DEFINE_INT32(SER_MXS_BAUD, 5);
@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
@@ -0,0 +1,6 @@
+# Sagetech SDK
+The contents of this folder includes selected functions from the full Sagetech SDK which can be found here:
+https://github.com/Sagetech-Avionics/sagetech-mxs-sdk
+
+## Links
+[Host Interface Control Document for MXS](https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf)
@@ -0,0 +1,19 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file appendChecksum.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 2, 2021
+ *
+ */
+
+#include "sgUtil.h"
+
+/*
+ * Documented in the header file
+ */
+void appendChecksum(uint8_t *buffer, uint8_t len)
+{
+	buffer[len - 1] = calcChecksum(buffer, len);
+}
@@ -0,0 +1,26 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file calcChecksum.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 2, 2021
+ *
+ */
+
+#include "sgUtil.h"
+
+/*
+ * Documented in the header file
+ */
+uint8_t calcChecksum(uint8_t *buffer, uint8_t len)
+{
+	uint8_t sum = 0x00;
+
+	// Add all bytes excluding checksum
+	for (uint8_t i = 0; i < len - 1; ++i) {
+		sum += buffer[i];
+	}
+
+	return sum;
+}
@@ -0,0 +1,22 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file charArray2Buf.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 2, 2021
+ *
+ */
+
+#include "sgUtil.h"
+#include <ctype.h>
+
+/*
+ * Documented in the header file.
+ */
+void charArray2Buf(uint8_t *bufferPos, char arr[], uint8_t len)
+{
+	for (uint8_t i = 0; i < len; ++i) {
+		bufferPos[i] = toupper(arr[i]);
+	}
+}
@@ -0,0 +1,30 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file float2Buf.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 2, 2021
+ *
+ */
+
+#include "sgUtil.h"
+
+/*
+ * Documented in the header file.
+ */
+void float2Buf(uint8_t *bufferPos, float value)
+{
+	const uint16_t FLOAT_SIZE = 4;
+
+	union {
+		float val;
+		unsigned char bytes[FLOAT_SIZE];
+	} conversion;
+
+	conversion.val = value;
+
+	for (int i = 0; i < FLOAT_SIZE; ++i) {
+		bufferPos[i] = conversion.bytes[i];
+	}
+}
@@ -0,0 +1,21 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file icao2Buf.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 2, 2021
+ *
+ */
+
+#include "sgUtil.h"
+
+/*
+ * Documented in the header file.
+ */
+void icao2Buf(uint8_t *bufferPos, uint32_t icao)
+{
+	bufferPos[0] = (icao & 0x00FF0000) >> 16;
+	bufferPos[1] = (icao & 0x0000FF00) >> 8;
+	bufferPos[2] = (icao & 0x000000FF);
+}
@@ -0,0 +1,13 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "sg.h"
+#include "sgUtil.h"
+
+#ifdef __cplusplus
+}
+#endif
@@ -0,0 +1,899 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file   sg.h
+ * @author jimb
+ *
+ * @date   Feb 10, 2021
+ *
+ * Sagetech protocol for host message building and parsing.
+ *
+ * This module performs both the following:
+ *    1. Parses raw Sagetech host messages defined in the SDIM and
+ *       returns a populated struct dataset of the message type.
+ *    2. Receives a populated struct dataset of the desired host message
+ *       and returns the corresponding raw message data buffer.
+ */
+
+#ifndef SG_H
+#define SG_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/// Host Message Lengths (bytes)
+#define SG_MSG_LEN_INSTALL 41
+#define SG_MSG_LEN_FLIGHT 17
+#define SG_MSG_LEN_OPMSG 17
+#define SG_MSG_LEN_GPS 68
+#define SG_MSG_LEN_DATAREQ 9
+#define SG_MSG_LEN_TARGETREQ 12
+#define SG_MSG_LEN_MODE 10
+
+/// Host Message Types
+#define SG_MSG_TYPE_HOST_INSTALL 0x01
+#define SG_MSG_TYPE_HOST_FLIGHT 0x02
+#define SG_MSG_TYPE_HOST_OPMSG 0x03
+#define SG_MSG_TYPE_HOST_GPS 0x04
+#define SG_MSG_TYPE_HOST_DATAREQ 0x05
+#define SG_MSG_TYPE_HOST_TARGETREQ 0x0B
+#define SG_MSG_TYPE_HOST_MODE 0x0C
+
+/// XPNDR Message Types
+#define SG_MSG_TYPE_XPNDR_ACK 0x80
+#define SG_MSG_TYPE_XPNDR_INSTALL 0x81
+#define SG_MSG_TYPE_XPNDR_FLIGHT 0x82
+#define SG_MSG_TYPE_XPNDR_STATUS 0x83
+#define SG_MSG_TYPE_XPNDR_COMMA 0x85
+#define SG_MSG_TYPE_XPNDR_MODE 0x8C
+#define SG_MSG_TYPE_XPNDR_VERSION 0x8E
+#define SG_MSG_TYPE_XPNDR_SERIALNUM 0x8F
+
+/// ADS-B Message Types
+#define SG_MSG_TYPE_ADSB_TSUMMARY 0x90
+#define SG_MSG_TYPE_ADSB_SVR 0x91
+#define SG_MSG_TYPE_ADSB_MSR 0x92
+#define SG_MSG_TYPE_ADSB_TSTATE 0x97
+#define SG_MSG_TYPE_ADSB_ARVR 0x98
+
+/// Start byte for all host messages
+#define SG_MSG_START_BYTE 0xAA
+
+/// Emitter category set byte values
+#define SG_EMIT_GROUP_A 0x00
+#define SG_EMIT_GROUP_B 0x01
+#define SG_EMIT_GROUP_C 0x02
+#define SG_EMIT_GROUP_D 0x03
+
+/// Emitter category enumeration offsets
+#define SG_EMIT_OFFSET_A 0x00
+#define SG_EMIT_OFFSET_B 0x10
+#define SG_EMIT_OFFSET_C 0x20
+#define SG_EMIT_OFFSET_D 0x30
+
+/**
+ * Available COM port baud rates.
+ */
+typedef enum {
+	baud38400 = 0,
+	baud600,
+	baud4800,
+	baud9600,
+	baud28800,
+	baud57600,
+	baud115200,
+	baud230400,
+	baud19200,
+	baud460800,
+	baud921600
+} sg_baud_t;
+
+/**
+ * Transponder ethernet configuration
+ */
+typedef struct {
+	uint32_t ipAddress;  /// The transponder ip address
+	uint32_t subnetMask; /// The transponder subnet mask
+	uint16_t portNumber; /// The transponder port number
+} sg_ethernet_t;
+
+/**
+ * Available GPS integrity SIL values
+ */
+typedef enum {
+	silUnknown = 0,
+	silLow,
+	silMedium,
+	silHigh
+} sg_sil_t;
+
+/**
+ * Available GPS integrity SDA values
+ */
+typedef enum {
+	sdaUnknown = 0,
+	sdaMinor,
+	sdaMajor,
+	sdaHazardous
+} sg_sda_t;
+
+/**
+ * Available emitter types
+ */
+typedef enum {
+	aUnknown = SG_EMIT_OFFSET_A,
+	aLight,
+	aSmall,
+	aLarge,
+	aHighVortex,
+	aHeavy,
+	aPerformance,
+	aRotorCraft,
+	bUnknown = SG_EMIT_OFFSET_B,
+	bGlider,
+	bAir,
+	bParachutist,
+	bUltralight,
+	bUAV = SG_EMIT_OFFSET_B + 6,
+	bSpace,
+	cUnknown = SG_EMIT_OFFSET_C,
+	cEmergency,
+	cService,
+	cPoint,
+	cCluster,
+	cLine,
+	dUnknown = SG_EMIT_OFFSET_D
+} sg_emitter_t;
+
+/**
+ * Available aircraft sizes in meters
+ */
+typedef enum {
+	sizeUnknown = 0, /// Dimensions unknown
+	sizeL15W23,      /// Length <= 15m & Width <= 23m
+	sizeL25W28,      /// Length <= 25m & Width <= 28.5m
+	sizeL25W34,      /// Length <= 25m & Width <= 34m
+	sizeL35W33,      /// Length <= 35m & Width <= 33m
+	sizeL35W38,      /// Length <= 35m & Width <= 38m
+	sizeL45W39,      /// Length <= 45m & Width <= 39.5m
+	sizeL45W45,      /// Length <= 45m & Width <= 45m
+	sizeL55W45,      /// Length <= 55m & Width <= 45m
+	sizeL55W52,      /// Length <= 55m & Width <= 52m
+	sizeL65W59,      /// Length <= 65m & Width <= 59.5m
+	sizeL65W67,      /// Length <= 65m & Width <= 67m
+	sizeL75W72,      /// Length <= 75m & Width <= 72.5m
+	sizeL75W80,      /// Length <= 75m & Width <= 80m
+	sizeL85W80,      /// Length <= 85m & Width <= 80m
+	sizeL85W90       /// Length <= 85m & Width <= 90m
+} sg_size_t;
+
+/**
+ * Available aircraft maximum airspeeds
+ */
+typedef enum {
+	speedUnknown = 0, /// Max speed unknown
+	speed75kt,        /// 0 knots    < Max speed < 75 knots
+	speed150kt,       /// 75 knots   < Max speed < 150 knots
+	speed300kt,       /// 150 knots  < Max speed < 300 knots
+	speed600kt,       /// 300 knots  < Max speed < 600 knots
+	speed1200kt,      /// 600 knots  < Max speed < 1200 knots
+	speedGreater      /// 1200 knots < Max speed
+} sg_airspeed_t;
+
+/**
+ * Available antenna configurations
+ */
+typedef enum {
+	antBottom = 1, /// bottom antenna only
+	antBoth = 3    /// both top and bottom antennae
+} sg_antenna_t;
+
+/**
+ * The XPNDR Installation Message.
+ * Host --> XPNDR.
+ * XPNDR --> Host.
+ * Use 'strcpy(install.reg, "REGVAL1")' to assign the registration.
+ */
+typedef struct {
+	uint32_t icao;          /// The aircraft's ICAO address
+	char reg[8];            /// The aircraft's registration (left-justified alphanumeric characters padded with spaces)
+	sg_baud_t com0;         /// The baud rate for COM Port 0
+	sg_baud_t com1;         /// The baud rate for COM Port 1
+	sg_ethernet_t eth;      /// The ethernet configuration
+	sg_sil_t sil;           /// The gps integrity SIL parameter
+	sg_sda_t sda;           /// The gps integrity SDA parameter
+	sg_emitter_t emitter;   /// The platform's emitter type
+	sg_size_t size;         /// The platform's dimensions
+	sg_airspeed_t maxSpeed; /// The platform's maximum airspeed
+	int16_t altOffset;      /// The altitude encoder offset is a legacy field that should always = 0
+	sg_antenna_t antenna;   /// The antenna configuration
+	bool altRes100;         /// Altitude resolution. true = 100 foot, false = 25 foot
+	bool hdgTrueNorth;      /// Heading type. true = true north, false = magnetic north
+	bool airspeedTrue;      /// Airspeed type. true = true speed, false = indicated speed
+	bool heater;            /// true = heater enabled, false = heater disabled
+	bool wowConnected;      /// Weight on Wheels sensor. true = connected, false = not connected
+} sg_install_t;
+
+/**
+ * The XPNDR Flight ID Message.
+ * Host --> XPNDR.
+ * XPNDR --> Host.
+ * * Use 'strcpy(id.flightID, "FLIGHTNO")' to assign the flight identification.
+ */
+typedef struct {
+	char flightId[9]; /// The flight identification (left-justified alphanumeric characters padded with spaces)
+} sg_flightid_t;
+
+/**
+ * Available transponder operating modes. The enumerated values are
+ * offset from the host message protocol values.
+ */
+typedef enum {
+	modeOff = 0, /// 'Off' Mode:     Xpdr will not transmit
+	modeOn,      /// 'On' Mode:      Full functionality with Altitude = Invalid
+	modeStby,    /// 'Standby' Mode: Reply to lethal interrogations, only
+	modeAlt      /// 'Alt' Mode:     Full functionality
+} sg_op_mode_t;
+
+/**
+ * Available emergency status codes.
+ */
+typedef enum {
+	emergcNone = 0, /// No Emergency
+	emergcGeneral,  /// General Emergency
+	emergcMed,      /// Lifeguard/Medical Emergency
+	emergcFuel,     /// Minimum Fuel
+	emergcComm,     /// No Communications
+	emergcIntrfrc,  /// Unlawful Interference
+	emergcDowned    /// Downed Aircraft
+} sg_emergc_t;
+
+/**
+ * The XPNDR Operating Message.
+ * Host --> XPNDR.
+ */
+typedef struct {
+	uint16_t squawk;        /// 4-digit octal Mode A code
+	sg_op_mode_t opMode;    /// Operational mode
+	bool savePowerUp;       /// Save power-up state in non-volatile
+	bool enableSqt;         /// Enable extended squitters
+	bool enableXBit;        /// Enable the x-bit
+	bool milEmergency;      /// Broadcast a military emergency
+	sg_emergc_t emergcType; /// Enumerated civilian emergency type
+	bool identOn;           /// Set the identification switch = On
+	bool altUseIntrnl;      /// True = Report altitude from internal pressure sensor (will ignore other bits in the field)
+	bool altHostAvlbl;      /// True = Host Altitude is being provided
+	bool altRes25;          /// Host Altitude Resolution from install message, True = 25 ft, False = 100 ft
+	int32_t altitude;       /// Sea-level altitude in feet. Field is ignored when internal altitude is selected.
+	bool climbValid;        /// Climb rate is provided;
+	int16_t climbRate;      /// Climb rate in ft/min. Limits are +/- 16,448 ft/min.
+	bool headingValid;      /// Heading is valid.
+	double heading;         /// Heading in degrees
+	bool airspdValid;       /// Airspeed is valid.
+	uint16_t airspd;        /// Airspeed in knots.
+} sg_operating_t;
+
+/**
+ * Avaiable NACp values.
+ */
+typedef enum {
+	nacpUnknown, /// >= 18.52  km ( 10  nmile)
+	nacp10dot0,  /// <  18.52  km ( 10  nmile)
+	nacp4dot0,   /// <   7.408 km (  4  nmile)
+	nacp2dot0,   /// <   3.704 km (  2  nmile)
+	nacp1dot0,   /// <   1.852 km (  1  nmile)
+	nacp0dot5,   /// <   0.926 km (0.5  nmile)
+	nacp0dot3,   /// <   0.556 km (0.3  nmile)
+	nacp0dot1,   /// <   0.185 km (0.1  nmile)
+	nacp0dot05,  /// <    92.6 m  (0.05 nmile)
+	nacp30,      /// <    30.0 m
+	nacp10,      /// <    10.0 m
+	nacp3        /// <     3.0 m
+} sg_nacp_t;
+
+/**
+ * Available NACv values (m/s)
+ */
+typedef enum {
+	nacvUnknown = 0, /// 10   <= NACv (or NACv is unknown)
+	nacv10dot0,      ///  3   <= NACv < 10
+	nacv3dot0,       ///  1   <= NACv <  3
+	nacv1dot0,       ///  0.3 <= NACv <  1
+	nacv0dot3        ///  0.0 <= NACv <  0.3
+} sg_nacv_t;
+
+/**
+ * The XPNDR Simulated GPS Message.
+ * Host --> XPNDR.
+ */
+typedef struct {
+	char longitude[12]; /// The absolute value of longitude (degree and decimal minute)
+	char latitude[11];  /// The absolute value of latitude (degree and decimal minute)
+	char grdSpeed[7];   /// The GPS over-ground speed (knots)
+	char grdTrack[9];   /// The GPS track referenced from True North (degrees, clockwise)
+	bool latNorth;      /// The aircraft is in the northern hemisphere
+	bool lngEast;       /// The aircraft is in the eastern hemisphere
+	bool fdeFail;       /// True = A satellite error has occurred
+	bool gpsValid;      /// True = GPS data is valid
+	char timeOfFix[11]; /// Time, relative to midnight UTC (can optionally be filled spaces)
+	float height;       /// The height above the WGS-84 ellipsoid (meters)
+	float hpl;          /// The Horizontal Protection Limit (meters)
+	float hfom;         /// The Horizontal Figure of Merit (meters)
+	float vfom;         /// The Vertical Figure of Merit (meters)
+	sg_nacv_t nacv;     /// Navigation Accuracy for Velocity (meters/second)
+} sg_gps_t;
+
+/**
+ * Available data request types
+ */
+typedef enum {
+	dataInstall = 0x81,    /// Installation data
+	dataFlightID = 0x82,   /// Flight Identification data
+	dataStatus = 0x83,     /// Status Response data
+	dataMode = 0x8C,       /// Mode Settings data
+	dataHealth = 0x8D,     /// Health Monitor data
+	dataVersion = 0x8E,    /// Version data
+	dataSerialNum = 0x8F,  /// Serial Number data
+	dataTOD = 0xD2,        /// Time of Day data
+	dataMode5 = 0xD3,      /// Mode 5 Indication data
+	dataCrypto = 0xD4,     /// Crypto Status data
+	dataMilSettings = 0xD7 /// Military Settings data
+} sg_datatype_t;
+
+/**
+ * The Data Request message.
+ * Host --> XPDR.
+ */
+typedef struct {
+	sg_datatype_t reqType; /// The desired data response
+	uint8_t resv[3];
+} sg_datareq_t;
+
+/**
+ * Available target request types
+ */
+typedef enum {
+	reportAuto = 0, /// Enable auto output of all target reports
+	reportSummary,  /// Report list of all tracked targets (disables auto-output)
+	reportIcao,     /// Generate reports for specific target, only (disables auto-output)
+	reportNone      /// Disable all target reports
+} sg_reporttype_t;
+
+/**
+ * Available target report transmission ports
+ */
+typedef enum {
+	transmitSource = 0, /// Transmit reports on channel where target request was received
+	transmitCom0,       /// Transmit reports on Com0
+	transmitCom1,       /// Transmit reports on Com1
+	transmitEth         /// Transmit reports on Ethernet
+} sg_transmitport_t;
+
+/**
+ * The Target Request message for ADS-B 'in' data.
+ * Host --> XPDR.
+ */
+typedef struct {
+	sg_reporttype_t reqType;        /// The desired report mode
+	sg_transmitport_t transmitPort; /// The communication port used for report transmission
+	uint16_t maxTargets;            /// The maximum number of targets to track (max value: 404)
+	uint32_t icao;                  /// The desired target's ID, if applicable
+	bool stateVector;               /// Transmit state vector reports
+	bool modeStatus;                /// Transmit mode status reports
+	bool targetState;               /// Transmit target state reports
+	bool airRefVel;                 /// Transmit air referenced velocity reports
+	bool tisb;                      /// Transmit raw TIS-B message reports (requires auto-output)
+	bool military;                  /// Enable tracking of military aircraft
+	bool commA;                     /// Transmit Comm-A Reports (requires auto-output)
+	bool ownship;                   /// Transmit reports about own aircraft
+} sg_targetreq_t;
+
+/**
+ * The Mode message.
+ * Host --> XPDR.
+ */
+typedef struct {
+	bool reboot; /// Reboot the MX
+} sg_mode_t;
+
+/**
+ * The XPNDR Acknowledge Message following all host messages.
+ * XPNDR --> Host.
+ */
+typedef struct {
+	uint8_t ackType;     /// Message type being acknowledged
+	uint8_t ackId;       /// Message ID being acknowledged
+	bool failXpdr;       /// Built-in-test failure
+	bool failSystem;     /// Required system input missing
+	bool failCrypto;     /// Crypto status failure
+	bool wow;            /// Weight-on-wheels indicates aircraft is on-ground
+	bool maint;          /// Maintenance mode enabled
+	bool isHostAlt;      /// False = Pressure sensor altitude, True = Host provided value
+	sg_op_mode_t opMode; /// Operational mode
+	int32_t alt;         /// Altitude (feet)
+	bool altValid;       /// Altitude is valid
+} sg_ack_t;
+
+/**
+ * The XPNDR Status Response Message following a Data Request for Status.
+ * XPNDR --> Host.
+ */
+typedef struct {
+	uint8_t versionSW; /// SW Version # installed on the XPNDR
+	uint8_t versionFW; /// FW Version # installed on the XPNDR
+	uint32_t crc;      /// CRC Checksum for the installed XPNDR SW/FW versions
+
+	bool powerUp : 1;    /// Integrity of CPU and Non-Volatile data at power-up
+	bool continuous : 1; /// Set by any other B.I.T. failures during operation
+	bool processor : 1;  /// One-time processor instruction set test at power-up
+	bool crcValid : 1;   /// Calculate then verifies the CRC against the stored value
+	bool memory : 1;     /// Processor RAM is functional
+	bool calibrated : 1; /// Transponder is calibrated
+	bool receiver : 1;   /// RF signals travel through hardware correctly
+	bool power53v : 1;   /// Voltage at the 53V power supply is correct
+	bool adc : 1;        /// Analog-to-Digital Converter is functional
+	bool pressure : 1;   /// Internal pressure transducer is functional
+	bool fpga : 1;       /// FPGA I/O operations are functional
+	bool rxLock : 1;     /// Rx oscillator reporting PLL Lock at reference frequency
+	bool txLock : 1;     /// Tx oscillator reporting PLL Lock at reference frequency
+	bool mtSuppress : 1; /// Mutual suppression is operating correctly
+	bool temp : 1;       /// Internal temperature is within range (< 110 C)
+	bool sqMonitor : 1;  /// Squitters are transmitting at their nominal rates
+	bool txRate : 1;     /// Transmission duty cycle is in the safe range
+	bool sysLatency : 1; /// Systems events occurred within expected time limits
+	bool txPower : 1;    /// Transmission power is in-range
+	bool voltageIn : 1;  /// Input voltage is in-range (10V-32V)
+	bool icao : 1;       /// ICAO Address is valid (fail at '000000' or 'FFFFFF')
+	bool gps : 1;        /// Valid GPS data is received at 1Hz, minimum
+} sg_status_t;
+
+/**
+ * The XPNDR Health Monitor Response Message.
+ * XPNDR --> Host.
+ */
+typedef struct {
+	int8_t socTemp; /// System on a Chip temperature
+	int8_t rfTemp;  /// RF Board temperature
+	int8_t ptTemp;  /// Pressure Transducer temperature
+} sg_healthmonitor_t;
+
+/**
+ * The XPNDR Version Response Message.
+ * XPNDR --> Host.
+ */
+typedef struct {
+	uint8_t swVersion;      /// The SW Version major revision number
+	uint8_t fwVersion;      /// The FW Version major revision number
+	uint16_t swSvnRevision; /// The SW Repository version number
+	uint16_t fwSvnRevision; /// The FW Repository version number
+} sg_version_t;
+
+/**
+ * The XPNDR Serial Number Response Message.
+ * XPNDR --> Host.
+ */
+typedef struct {
+	char ifSN[33];    /// The Interface Board serial number
+	char rfSN[33];    /// The RF Board serial number
+	char xpndrSN[33]; /// The Transponder serial number
+} sg_serialnumber_t;
+
+/// The state vector report type.
+typedef enum {
+	svrAirborne = 1, /// Airborne state vector report type.
+	svrSurface       /// Surface state vector report type.
+} sg_svr_type_t;
+
+/// The state vector report participant address type.
+typedef enum {
+	svrAdrIcaoUnknown,    /// ICAO address unknown emitter category.
+	svrAdrNonIcaoUnknown, /// Non-ICAO address unknown emitter category.
+	svrAdrIcao,           /// ICAO address aircraft.
+	svrAdrNonIcao,        /// Non-ICAO address aircraft.
+	svrAdrIcaoSurface,    /// ICAO address surface vehicle, fixed ground, tethered obstruction.
+	svrAdrNonIcaoSurface, /// Non-ICAO address surface vehicle, fixed ground, tethered obstruction.
+	svrAdrDup,            /// Duplicate target of another ICAO address.
+	svrAdrAdsr            /// ADS-R target.
+} sg_addr_type_t;
+
+/// The surface part of a state vector report.
+typedef struct {
+	int16_t speed;   /// Surface speed.
+	int16_t heading; /// Surface heading.
+} sg_svr_surface_t;
+
+/// The airborne part of a state vector report.
+typedef struct {
+	int16_t velNS;   /// The NS speed vector component. [knots]
+	int16_t velEW;   /// The EW speed vector component. [knots]
+	int16_t speed;   /// Speed from N/S and E/W velocity. [knots]
+	int16_t heading; /// Heading from N/S and E/W velocity. [deg from N]
+	int32_t geoAlt;  /// Geometric altitude. [ft]
+	int32_t baroAlt; /// Barometric altitude. [ft]
+	int16_t vrate;   /// Vertical rate. [ft/min]
+	float estLat;    /// Estimated latitude. [deg N]
+	float estLon;    /// Estimated longitude. [deg E]
+} sg_svr_airborne_t;
+
+typedef struct {
+	bool baroVRate : 1;   /// Barometric vertical rate valid.
+	bool geoVRate : 1;    /// Geometric vertical rate valid.
+	bool baroAlt : 1;     /// Barometric altitude valid.
+	bool surfHeading : 1; /// Surface heading valid.
+	bool surfSpeed : 1;   /// Surface speed valid.
+	bool airSpeed : 1;    /// Airborne speed and heading valid.
+	bool geoAlt : 1;      /// Geometric altitude valid.
+	bool position : 1;    /// Lat and lon data valid.
+} sg_svr_validity_t;
+
+typedef struct {
+	uint8_t reserved : 6; /// Reserved.
+	bool estSpeed : 1;    /// Estimated N/S and E/W velocity.
+	bool estPosition : 1; /// Estimated lat/lon position.
+} sg_svr_est_validity_t;
+
+/**
+ * The XPDR ADS-B state vector report Message.
+ * Host --> XPDR.
+ *
+ * @note The time of applicability values are based on the MX system clock that starts
+ * at 0 on power up. The time is the floating point number that is the seconds since
+ * power up. The time number rolls over at 512.0.
+ */
+typedef struct {
+	sg_svr_type_t type; /// Report type.
+	union {
+		uint8_t flags;
+		sg_svr_validity_t validity; /// Field validity flags.
+	};
+	union {
+		uint8_t eflags;
+		sg_svr_est_validity_t evalidity; /// Estimated field validity flags.
+	};
+	uint32_t addr;           /// Participant address.
+	sg_addr_type_t addrType; /// Participant address type.
+	float toaEst;            /// Report estimated position and speed time of applicability.
+	float toaPosition;       /// Report position time of applicability.
+	float toaSpeed;          /// Report speed time of applicability.
+	uint8_t survStatus;      /// Surveillance status.
+	uint8_t mode;            /// Report mode.
+	uint8_t nic;             /// Navigation integrity category.
+	float lat;               /// Latitude.
+	float lon;               /// Longitude.
+	union {
+		sg_svr_surface_t surface;   /// Surface SVR data.
+		sg_svr_airborne_t airborne; /// Airborne SVR data.
+	};
+} sg_svr_t;
+
+typedef enum {
+	msrTypeV0,
+	msrTypeV1Airborne,
+	msrTypeV1Surface,
+	msrTypeV2Airborne,
+	msrTypeV2Surface
+} sg_msr_type_t;
+
+typedef struct {
+	uint8_t reserved : 2;
+	bool priority : 1;
+	bool sil : 1;
+	bool nacv : 1;
+	bool nacp : 1;
+	bool opmode : 1;
+	bool capcodes : 1;
+} sg_msr_validity_t;
+
+typedef enum {
+	adsbVerDO260,
+	adsbVerDO260A,
+	adsbVerDO260B
+} sg_adsb_version_t;
+
+typedef enum {
+	adsbUnknown,
+	adsbLight,
+	adsbSmall = 0x3,
+	adsbLarge = 0x5,
+	adsbHighVortex,
+	adsbHeavy,
+	adsbPerformance,
+	adsbRotorcraft = 0x0A,
+	adsbGlider,
+	adsbAir,
+	adsbUnmaned,
+	adsbSpace,
+	adsbUltralight,
+	adsbParachutist,
+	adsbVehicle_emg = 0x14,
+	adsbVehicle_serv,
+	adsbObsticlePoint,
+	adsbObsticleCluster,
+	adsbObsticleLinear
+} sg_adsb_emitter_t;
+
+typedef enum {
+	priNone,
+	priGeneral,
+	priMedical,
+	priFuel,
+	priComm,
+	priUnlawful,
+	priDowned
+} sg_priority_t;
+
+typedef enum {
+	tcrNone,
+	tcrSingle,
+	tcrMultiple
+} sg_tcr_t;
+
+typedef struct {
+	bool b2low : 1;
+	bool uat : 1;
+	bool arv : 1;
+	bool tsr : 1;
+	bool adsb : 1;
+	bool tcas : 1;
+	sg_tcr_t tcr;
+} sg_capability_t;
+
+typedef enum {
+	gpsLonNodata,
+	gpsLonSensorSupplied,
+	gpsLon2m,
+	gpsLon4m,
+	gpsLon6m,
+	gpsLon8m,
+	gpsLon10m,
+	gpsLon12m,
+	gpsLon14m,
+	gpsLon16m,
+	gpsLon18m,
+	gpsLon20m,
+	gpsLon22m,
+	gpsLon24m,
+	gpsLon26m,
+	gpsLon28m,
+	gpsLon30m,
+	gpsLon32m,
+	gpsLon34m,
+	gpsLon36m,
+	gpsLon38m,
+	gpsLon40m,
+	gpsLon42m,
+	gpsLon44m,
+	gpsLon46m,
+	gpsLon48m,
+	gpsLon50m,
+	gpsLon52m,
+	gpsLon54m,
+	gpsLon56m,
+	gpsLon58m,
+	gpsLon60m
+} sg_gps_lonofs_t;
+
+typedef enum {
+	gpslatNodata,
+	gpslatLeft2m,
+	gpslatLeft4m,
+	gpslatLeft6m,
+	gpslatRight0m,
+	gpslatRight2m,
+	gpslatRight4m,
+	gpslatRight6m,
+} sg_gps_latofs_t;
+
+typedef struct {
+	bool gpsLatFmt;
+	sg_gps_latofs_t gpsLatOfs;
+	bool gpsLonFmt;
+	sg_gps_lonofs_t gpsLonOfs;
+	bool tcasRA : 1;
+	bool ident : 1;
+	bool singleAnt : 1;
+} sg_adsb_opmode_t;
+
+typedef enum {
+	gvaUnknown,
+	gvaLT150m,
+	gvaLT45m
+} sg_gva_t;
+
+typedef enum {
+	nicGolham,
+	nicNonGilham
+} sg_nicbaro_t;
+
+typedef enum {
+	svsilUnknown,
+	svsilPow3,
+	svsilPow5,
+	svsilPow7
+} sg_svsil_t;
+
+typedef struct {
+	sg_nacp_t nacp;
+	sg_nacv_t nacv;
+	sg_sda_t sda;
+	bool silSupp;
+	sg_svsil_t sil;
+	sg_gva_t gva;
+	sg_nicbaro_t nicBaro;
+} sg_sv_qual_t;
+
+typedef enum {
+	trackTrueNorth,
+	trackMagNorth,
+	headingTrueNorth,
+	headingMagNorth
+} sg_trackheading_t;
+
+typedef enum {
+	vrateBaroAlt,
+	vrateGeoAlt
+} sg_vratetype_t;
+
+/**
+ * The XPDR ADS-B mode status report Message.
+ * Host --> XPDR.
+ *
+ * @note The time of applicability values are based on the MX system clock that starts
+ * at 0 on power up. The time is the floating point number that is the seconds since
+ * power up. The time number rolls over at 512.0.
+ */
+typedef struct {
+	sg_msr_type_t type; /// Report type.
+
+	union {
+		uint8_t flags;
+		sg_msr_validity_t validity; /// Field validity flags.
+	};
+
+	uint32_t addr;           /// Participant address.
+	sg_addr_type_t addrType; /// Participant address type.
+
+	float toa;
+	sg_adsb_version_t version;
+	char callsign[9];
+	sg_adsb_emitter_t emitter;
+	sg_size_t size;
+	sg_priority_t priority;
+	sg_capability_t capability;
+	sg_adsb_opmode_t opMode;
+	sg_sv_qual_t svQuality;
+	sg_trackheading_t trackHeading;
+	sg_vratetype_t vrateType;
+} sg_msr_t;
+
+/**
+ * Convert install message struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw install message.
+ * @param[in]  stl    The install message struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in stl parameter must be pre-validated.
+ */
+bool sgEncodeInstall(uint8_t *buffer, sg_install_t *stl, uint8_t msgId);
+
+/**
+ * Convert flight identification struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw flight identification message.
+ * @param[in]  id     The flight id struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in id parameter must be pre-validated.
+ */
+bool sgEncodeFlightId(uint8_t *buffer, sg_flightid_t *id, uint8_t msgId);
+
+/**
+ * Convert operating message struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw operating message.
+ * @param[in]  op     The operating message struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in op parameter must be pre-validated.
+ */
+bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId);
+
+/* TODO: Create GPS helper functions to convert other data types --> char buffers */
+
+/**
+ * Convert GPS message struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw GPS message.
+ * @param[in]  gps    The GPS message struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in gps parameter must be pre-validated.
+ */
+bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId);
+
+/**
+ * Convert data request message struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw target request message.
+ * @param[in]  data   The data request message struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in data parameter must be pre-validated.
+ */
+bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId);
+
+/**
+ * Convert target request message struct to the raw buffer format.
+ *
+ * @param[out] buffer An empty buffer to contain the raw target request message.
+ * @param[in]  tgt    The target request message struct with fields populated.
+ * @param[in]  msgId  The sequence number for the message.
+ *
+ * @return true if successful or false on failure.
+ *
+ * @warning data in tgt parameter must be pre-validated.
+ */
+bool sgEncodeTargetReq(uint8_t *buffer, sg_targetreq_t *tgt, uint8_t msgId);
+
+/**
+ * Process the ACK message response from the transponder.
+ *
+ * @param[in]  buffer The raw ACK message buffer.
+ * @param[out] ack    The parsed message results.
+ *
+ * @return true if successful or false on failure.
+ */
+bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack);
+
+/**
+ * Process the Install message response from the transponder.
+ *
+ * @param[in]  buffer The raw Install message buffer.
+ * @param[out] stl    The parsed message results.
+ *
+ * @return true if successful or false on failure.
+ */
+bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl);
+
+/**
+ * Process the Flight ID message response from the transponder.
+ *
+ * @param[in]  buffer The raw Flight ID message buffer.
+ * @param[out] id     The parsed message results.
+ *
+ * @return true if successful or false on failure.
+ */
+bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id);
+
+/**
+ * Process the state vector report message.
+ *
+ * @param[in]  buffer The raw SVR message buffer.
+ * @param[out] svr    The parsed SVR message.
+ *
+ * @return true if successful or false on failure.
+ */
+bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr);
+
+/**
+ * Process the mode status report message.
+ *
+ * @param buffer The raw MSR message buffer.
+ * @param msr    The parsed MSR message.
+ *
+ * @return true if successful or false on failure.
+ */
+bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr);
+
+#endif /* SG_H */
@@ -0,0 +1,72 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file   sgDecodeAck.c
+ * @author jimb
+ *
+ * @date   Feb 10, 2021
+ *
+ * This file receives a raw Acknowledge message buffer and
+ * parses the payload into a data struct.
+ */
+
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
+
+#include "sg.h"
+#include "sgUtil.h"
+
+#define SG_ACK_XPNDR_FAIL 0x01
+#define SG_ACK_SYSTEM_FAIL 0x02
+#define SG_ACK_CRYPTO_FAIL 0x04
+#define SG_ACK_WOW 0x08
+#define SG_ACK_MAINT 0x10
+#define SG_ACK_ALT_SOURCE 0x20
+#define SG_ACK_OP_MODE 0xC0
+
+typedef struct __attribute__((packed))
+{
+	uint8_t start;
+	uint8_t type;
+	uint8_t id;
+	uint8_t payloadLen;
+	uint8_t ackType;
+	uint8_t ackId;
+	uint8_t state;
+	uint8_t alt[3];
+	uint8_t checksum;
+} ack_t;
+
+/*
+ * Documented in the header file.
+ */
+bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack)
+{
+	ack_t sgAck;
+	memcpy(&sgAck, buffer, sizeof(ack_t));
+
+	ack->ackType = sgAck.ackType;
+	ack->ackId = sgAck.ackId;
+	ack->failXpdr = (sgAck.state & SG_ACK_XPNDR_FAIL) > 0;
+	ack->failSystem = (sgAck.state & SG_ACK_SYSTEM_FAIL) > 0;
+	ack->failCrypto = (sgAck.state & SG_ACK_CRYPTO_FAIL) > 0;
+	ack->wow = (sgAck.state & SG_ACK_WOW) > 0;
+	ack->maint = (sgAck.state & SG_ACK_MAINT) > 0;
+	ack->isHostAlt = (sgAck.state & SG_ACK_ALT_SOURCE) > 0;
+	ack->opMode = (sgAck.state & SG_ACK_OP_MODE) >> 6;
+
+	int32_t int24 = toInt32(sgAck.alt);
+
+	// Bitmask altitude field to determine if alt = invalid
+	if ((int24 & 0x00FFFFFF) == 0x00800000) {
+		ack->alt = 0;
+		ack->altValid = false;
+
+	} else {
+		ack->alt = int24;
+		ack->altValid = true;
+	}
+
+	return true;
+}
@@ -0,0 +1,41 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file sgDecodeFlightId.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 10, 2021
+ *
+ */
+
+#include <string.h>
+
+#include "sg.h"
+#include "sgUtil.h"
+
+#define SG_ID_LEN 8 // The number of bytes in the flight id field
+
+typedef struct __attribute__((packed))
+{
+	uint8_t start;
+	uint8_t type;
+	uint8_t id;
+	uint8_t payloadLen;
+	char flightId[SG_ID_LEN];
+	uint8_t rsvd[4];
+	uint8_t checksum;
+} flightid_t;
+
+/*
+ * Documented in the header file.
+ */
+bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id)
+{
+	flightid_t sgId;
+	memcpy(&sgId, buffer, sizeof(flightid_t));
+
+	strcpy(id->flightId, sgId.flightId);
+	memset(&id->flightId[SG_ID_LEN], '\0', 1); // Ensure flight id is null-terminated
+
+	return true;
+}
@@ -0,0 +1,84 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file sgDecodeInstall.c
+ * @author Jacob.Garrison
+ *
+ * @date Mar 9, 2021
+ *
+ */
+
+#include <string.h>
+#include <stdbool.h>
+
+#include "sg.h"
+#include "sgUtil.h"
+
+#define SG_REG_LEN    7     // The number of bytes in the registration field
+
+#define SG_STL_ANTENNA      0x03
+#define SG_STL_ALT_RES      0x08
+#define SG_STL_HDG_TYPE     0x10
+#define SG_STL_SPD_TYPE     0x20
+#define SG_STL_HEATER       0x40
+#define SG_STL_WOW          0x80
+
+typedef struct __attribute__((packed))
+{
+	uint8_t  start;
+	uint8_t  type;
+	uint8_t  id;
+	uint8_t  payloadLen;
+	uint8_t  icao[3];
+	char     registration[SG_REG_LEN];
+	uint8_t  rsvd1[2];
+	uint8_t  com0;
+	uint8_t  com1;
+	uint8_t  ipAddress[4];
+	uint8_t  subnetMask[4];
+	uint8_t  port[2];
+	uint8_t  gpsIntegrity;
+	uint8_t  emitterSet;
+	uint8_t  emitterType;
+	uint8_t  size;
+	uint8_t  maxSpeed;
+	uint8_t  altOffset[2];
+	uint8_t  rsvd2[2];
+	uint8_t  config;
+	uint8_t  rsvd3[2];
+	uint8_t  checksum;
+} stl_t;
+
+/*
+ * Documented in the header file.
+ */
+bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl)
+{
+	memset(&stl->reg[0], '\0', sizeof(stl->reg));  // Ensure registration is null-terminated
+
+	stl_t sgStl;
+	memcpy(&sgStl, buffer, sizeof(stl_t));
+
+	stl->icao           = toIcao(sgStl.icao);
+	strcpy(stl->reg,      sgStl.registration);
+	memset(&stl->reg[SG_REG_LEN], 0, 1);  // Ensure registration is null-terminated
+	stl->com0           = (sg_baud_t)(sgStl.com0);
+	stl->com1           = (sg_baud_t)(sgStl.com1);
+	stl->eth.ipAddress  = toUint32(sgStl.ipAddress);
+	stl->eth.subnetMask = toUint32(sgStl.subnetMask);
+	stl->eth.portNumber = toUint16(sgStl.port);
+	stl->sil            = (sg_sil_t)(sgStl.gpsIntegrity >> 4);
+	stl->sda            = (sg_sda_t)(sgStl.gpsIntegrity & 0x0F);
+	stl->emitter        = (sg_emitter_t)(0x10 * sgStl.emitterSet + sgStl.emitterType);
+	stl->size           = (sg_size_t)sgStl.size;
+	stl->maxSpeed       = (sg_airspeed_t)sgStl.maxSpeed;
+	stl->altOffset      = toUint16(sgStl.altOffset);
+	stl->antenna        = (sg_antenna_t)sgStl.config & SG_STL_ANTENNA;
+	stl->altRes100      = sgStl.config & SG_STL_ALT_RES;
+	stl->hdgTrueNorth   = sgStl.config & SG_STL_HDG_TYPE;
+	stl->airspeedTrue   = sgStl.config & SG_STL_SPD_TYPE;
+	stl->heater         = sgStl.config & SG_STL_HEATER;
+	stl->wowConnected   = sgStl.config & SG_STL_WOW;
+
+	return true;
+}
@@ -0,0 +1,183 @@
+/**
+ * @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved.
+ *
+ * @file sgDecodeMSR.c
+ * @author jim.billmeyer
+ *
+ * @date Mar 17, 2021
+ */
+
+#include "sg.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include "sgUtil.h"
+
+#define MS_PARAM_TOA (1 << 3)
+#define MS_PARAM_ADSBVER (1 << 2)
+#define MS_PARAM_CALLSIGN (1 << 1)
+#define MS_PARAM_CATEMITTER (1 << 0)
+
+#define MS_PARAM_AVLEN (1 << 7)
+#define MS_PARAM_PRIORITY (1 << 6)
+#define MS_PARAM_CAPCODES (1 << 5)
+#define MS_PARAM_OPMODE (1 << 4)
+#define MS_PARAM_NACP (1 << 3)
+#define MS_PARAM_NACV (1 << 2)
+#define MS_PARAM_SDA (1 << 1)
+#define MS_PARAM_GVA (1 << 0)
+
+#define MS_PARAM_NIC (1 << 7)
+#define MS_PARAM_HEADING (1 << 6)
+#define MS_PARAM_VRATE (1 << 5)
+
+#define MS_CAP_B2LOW (1 << 3)
+#define MS_CAP_UAT (1 << 1)
+#define MS_CAP_TCR ((1 << 2) | (1 << 3))
+#define MS_CAP_TSR (1 << 4)
+#define MS_CAP_ARV (1 << 5)
+#define MS_CAP_ADSB (1 << 6)
+#define MS_CAP_TCAS (1 << 7)
+
+#define MS_OP_GPS_LATFMT (1 << 7)
+#define MS_OP_GPS_LONFMT (1 << 6)
+#define MS_OP_TCAS_RA (1 << 5)
+#define MS_OP_IDENT (1 << 4)
+#define MS_OP_SINGLE_ANT (1 << 2)
+
+/// the payload offset.
+#define PBASE 4
+
+/*
+ * Documented in the header file.
+ */
+bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr)
+{
+	memset(msr, 0, sizeof(sg_msr_t));
+
+	uint8_t fields[3];
+	memcpy(fields, &buffer[PBASE + 0], 3);
+
+	if (buffer[PBASE + 1] == 0x6E && buffer[PBASE + 2] == 0x60) {
+		msr->type = msrTypeV0;
+
+	} else if (buffer[PBASE + 1] == 0x7E && buffer[PBASE + 2] == 0xE0) {
+		msr->type = msrTypeV1Airborne;
+
+	} else if (buffer[PBASE + 1] == 0xFE && buffer[PBASE + 2] == 0xE0) {
+		msr->type = msrTypeV1Surface;
+
+	} else if (buffer[PBASE + 1] == 0x7F && buffer[PBASE + 2] == 0xE0) {
+		msr->type = msrTypeV2Airborne;
+
+	} else if (buffer[PBASE + 1] == 0xFF && buffer[PBASE + 2] == 0xE0) {
+		msr->type = msrTypeV2Surface;
+	}
+
+	msr->flags = buffer[PBASE + 3];
+	msr->addr = toInt32(&buffer[PBASE + 4]) & 0xFFFFFF;
+	msr->addrType = buffer[PBASE + 7] & 0xFF;
+
+	uint8_t ofs = 8;
+
+	if (fields[0] & MS_PARAM_TOA) {
+		msr->toa = toTOA(&buffer[PBASE + ofs]);
+		ofs += 2;
+	}
+
+	if (fields[0] & MS_PARAM_ADSBVER) {
+		msr->version = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[0] & MS_PARAM_CALLSIGN) {
+		memset(msr->callsign, 0, 9);
+		memcpy(msr->callsign, &buffer[PBASE + ofs], 8);
+		ofs += 8;
+	}
+
+	if (fields[0] & MS_PARAM_CATEMITTER) {
+		msr->emitter = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_AVLEN) {
+		msr->size = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_PRIORITY) {
+		msr->priority = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_CAPCODES) {
+		uint8_t cap = buffer[PBASE + ofs + 0];
+		msr->capability.b2low = cap & MS_CAP_B2LOW;
+
+		cap = buffer[PBASE + ofs + 1];
+		msr->capability.uat = cap & MS_CAP_UAT;
+		msr->capability.tcr = (cap & MS_CAP_TCR) >> 2;
+		msr->capability.tsr = cap & MS_CAP_TSR;
+		msr->capability.arv = cap & MS_CAP_ARV;
+		msr->capability.adsb = cap & MS_CAP_ADSB;
+		msr->capability.tcas = cap & MS_CAP_TCAS;
+
+		ofs += 3;
+	}
+
+	if (fields[1] & MS_PARAM_OPMODE) {
+		uint8_t op = buffer[PBASE + ofs + 0];
+		msr->opMode.gpsLatFmt = (op & MS_OP_GPS_LATFMT) == 0;
+		msr->opMode.gpsLonFmt = (op & MS_OP_GPS_LONFMT) == 0;
+		msr->opMode.tcasRA = op & MS_OP_TCAS_RA;
+		msr->opMode.ident = op & MS_OP_IDENT;
+		msr->opMode.singleAnt = op & MS_OP_SINGLE_ANT;
+
+		op = buffer[PBASE + ofs + 1];
+		msr->opMode.gpsLatOfs = op >> 5;
+		msr->opMode.gpsLonOfs = op & 0x17;
+
+		ofs += 2;
+	}
+
+	if (fields[1] & MS_PARAM_NACP) {
+		msr->svQuality.nacp = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_NACV) {
+		msr->svQuality.nacv = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_SDA) {
+		uint8_t sda = buffer[PBASE + ofs];
+		msr->svQuality.sda = (sda & 0x18) >> 3;
+		msr->svQuality.silSupp = (sda & 0x04);
+		msr->svQuality.sil = (sda & 0x03);
+		ofs++;
+	}
+
+	if (fields[1] & MS_PARAM_GVA) {
+		msr->svQuality.gva = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[2] & MS_PARAM_NIC) {
+		msr->svQuality.nicBaro = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[2] & MS_PARAM_HEADING) {
+		msr->trackHeading = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	if (fields[2] & MS_PARAM_VRATE) {
+		msr->vrateType = buffer[PBASE + ofs];
+		ofs++;
+	}
+
+	return true;
+}
@@ -0,0 +1,209 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file   sgDecodeSVR.c
+ * @author jimb
+ *
+ * @date   Feb 10, 2021
+ *
+ * This file receives a raw ADS-B target state vector report message buffer and
+ * parses the payload into a data struct.
+ */
+
+#include <string.h>
+#include <stdbool.h>
+#include <string.h>
+#include <math.h>
+
+#include "sg.h"
+#include "sgUtil.h"
+#include "target.h"
+
+// airborne surface
+// -------- --------
+#define SV_PARAM_TOA_EPOS (1 << 3) // x
+#define SV_PARAM_TOA_POS (1 << 2)  // x        x
+#define SV_PARAM_TOA_VEL (1 << 1)  // x        x
+#define SV_PARAM_LATLON (1 << 0)   // x        x
+
+#define SV_PARAM_GEOALT (1 << 7)    // x
+#define SV_PARAM_VEL (1 << 6)       // x
+#define SV_PARAM_SURF_GS (1 << 5)   //          x
+#define SV_PARAM_SURF_HEAD (1 << 4) //          x
+#define SV_PARAM_BAROALT (1 << 3)   // x
+#define SV_PARAM_VRATE (1 << 2)     // x
+#define SV_PARAM_NIC (1 << 1)       // x        x
+#define SV_PARAM_ESTLAT (1 << 0)    // x
+
+#define SV_PARAM_ESTLON (1 << 7) // x
+#define SV_PARAM_ESTNVEL (1 << 6)
+#define SV_PARAM_ESTEVAL (1 << 5)
+#define SV_PARAM_SURV (1 << 4)   // x        x
+#define SV_PARAM_REPORT (1 << 3) // x        x
+
+/// the payload offset.
+#define PBASE 4
+
+/*
+ * Documented in the header file.
+ */
+bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr)
+{
+	//   memset(svr, 0, sizeof(sg_svr_t));
+
+	uint8_t fields[3];
+	memcpy(&fields, &buffer[PBASE + 0], 3);
+
+	svr->type = buffer[PBASE + 0] == 0x1F ? svrAirborne : svrSurface;
+	svr->flags = buffer[PBASE + 3];
+	svr->eflags = buffer[PBASE + 4];
+	svr->addr = toInt32(&buffer[PBASE + 5]) & 0xFFFFFF;
+	svr->addrType = buffer[PBASE + 8] & 0xFF;
+
+	uint8_t ofs = 9;
+
+	if (fields[0] & SV_PARAM_TOA_EPOS) {
+		svr->toaEst = toTOA(&buffer[PBASE + ofs]);
+		ofs += 2;
+	}
+
+	if (fields[0] & SV_PARAM_TOA_POS) {
+		svr->toaPosition = toTOA(&buffer[PBASE + ofs]);
+		ofs += 2;
+	}
+
+	if (fields[0] & SV_PARAM_TOA_VEL) {
+		svr->toaSpeed = toTOA(&buffer[PBASE + ofs]);
+		ofs += 2;
+	}
+
+	if (fields[0] & SV_PARAM_LATLON) {
+		if (svr->validity.position) {
+			svr->lat = toLatLon(&buffer[PBASE + ofs + 0]);
+			svr->lon = toLatLon(&buffer[PBASE + ofs + 3]);
+
+		} else {
+			svr->lat = 0.0;
+			svr->lon = 0.0;
+		}
+
+		ofs += 6;
+	}
+
+	if (svr->type == svrAirborne) {
+		if (fields[1] & SV_PARAM_GEOALT) {
+			if (svr->validity.geoAlt) {
+				svr->airborne.geoAlt = toAlt(&buffer[PBASE + ofs]);
+
+			} else {
+				svr->airborne.geoAlt = 0;
+			}
+
+			ofs += 3;
+		}
+
+		if (fields[1] & SV_PARAM_VEL) {
+			if (svr->validity.airSpeed) {
+				int16_t nvel = toVel(&buffer[PBASE + ofs + 0]);
+				int16_t evel = toVel(&buffer[PBASE + ofs + 2]);
+
+				svr->airborne.heading = toHeading2((double)nvel, (double)evel);
+				svr->airborne.speed = sqrt(nvel * nvel + evel * evel);
+				svr->airborne.velEW = evel;
+				svr->airborne.velNS = nvel;
+
+			} else {
+				svr->airborne.heading = 0;
+				svr->airborne.speed = 0;
+				svr->airborne.velEW = 0;
+				svr->airborne.velNS = 0;
+			}
+
+			ofs += 4;
+		}
+
+		if (fields[1] & SV_PARAM_BAROALT) {
+			if (svr->validity.baroAlt) {
+				svr->airborne.baroAlt = toAlt(&buffer[PBASE + ofs]);
+
+			} else {
+				svr->airborne.baroAlt = 0;
+			}
+
+			ofs += 3;
+		}
+
+		if (fields[1] & SV_PARAM_VRATE) {
+			if (svr->validity.baroVRate || svr->validity.geoVRate) {
+				svr->airborne.vrate = toInt16(&buffer[PBASE + ofs]);
+
+			} else {
+				svr->airborne.vrate = 0;
+			}
+
+			ofs += 2;
+		}
+
+	} else {
+		if (fields[1] & SV_PARAM_SURF_GS) {
+			if (svr->validity.surfSpeed) {
+				svr->surface.speed = toGS(&buffer[PBASE + ofs]);
+
+			} else {
+				svr->surface.speed = 0;
+			}
+
+			ofs += 1;
+		}
+
+		if (fields[1] & SV_PARAM_SURF_HEAD) {
+			if (svr->validity.surfHeading) {
+				svr->surface.heading = toHeading(&buffer[PBASE + ofs]);
+
+			} else {
+				svr->surface.heading = 0;
+			}
+
+			ofs += 1;
+		}
+	}
+
+	if (fields[1] & SV_PARAM_NIC) {
+		svr->nic = buffer[PBASE + ofs];
+
+		ofs += 1;
+	}
+
+	if (fields[1] & SV_PARAM_ESTLAT) {
+		if (svr->evalidity.estPosition) {
+			svr->airborne.estLat = toLatLon(&buffer[PBASE + ofs]);
+
+		} else {
+			svr->airborne.estLat = 0;
+		}
+
+		ofs += 3;
+	}
+
+	if (fields[2] & SV_PARAM_ESTLON) {
+		if (svr->evalidity.estPosition) {
+			svr->airborne.estLon = toLatLon(&buffer[PBASE + ofs]);
+
+		} else {
+			svr->airborne.estLon = 0;
+		}
+
+		ofs += 3;
+	}
+
+	if (fields[2] & SV_PARAM_SURV) {
+		svr->survStatus = buffer[PBASE + ofs];
+		ofs += 1;
+	}
+
+	if (fields[2] & SV_PARAM_REPORT) {
+		svr->mode = buffer[PBASE + ofs];
+	}
+
+	return true;
+}
@@ -0,0 +1,51 @@
+/**
+ * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
+ *
+ * @file sgEncodeDataReq.c
+ * @author Jacob.Garrison
+ *
+ * @date Feb 23, 2021
+ *
+ * This file receives a populated data request struct and
+ * converts it into a data request message buffer.
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include "sg.h"
+#include "sgUtil.h"
+
+#define SG_PAYLOAD_LEN_DATAREQ SG_MSG_LEN_DATAREQ - 5 /// the payload length.
+
+#define PBASE 4 /// the payload offset.
+
+#define OFFSET_REQ_TYPE 0 /// the requested response message type
+#define OFFSET_RSVD_1 1   /// a reserved field
+#define OFFSET_RSVD_2 2   /// a reserved field
+#define OFFSET_RSVD_3 3   /// a reserved field
+
+/*
+ * Documented in the header file.
+ */
+bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId)
+{
+	// populate header
+	buffer[0] = SG_MSG_START_BYTE;
+	buffer[1] = SG_MSG_TYPE_HOST_DATAREQ;
+	buffer[2] = msgId;
+	buffer[3] = SG_PAYLOAD_LEN_DATAREQ;
+
+	// populate Request Type
+	buffer[PBASE + OFFSET_REQ_TYPE] = data->reqType;
+
+	// populate Reserved fields
+	buffer[PBASE + OFFSET_RSVD_1] = 0;
+	buffer[PBASE + OFFSET_RSVD_2] = 0;
+	buffer[PBASE + OFFSET_RSVD_3] = 0;
+
+	// populate checksum
+	appendChecksum(buffer, SG_MSG_LEN_DATAREQ);
+
+	return true;
+}
--- a/Show More
+++ b/Show More