diff --git a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
index 8bcbbacb5a..d8f683a3db 100644
--- a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
+++ b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017  Intel Corporation. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 #include "ArmAuthorization.h"
 
 #include <string.h>
diff --git a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.h b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.h
index 31a8bcc518..3c25e6fd02 100644
--- a/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.h
+++ b/apps/controller/commander/Arming/ArmAuthorization/ArmAuthorization.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017  Intel Corporation. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
diff --git a/apps/controller/fw_att_control/ecl_controller.cpp b/apps/controller/fw_att_control/ecl_controller.cpp
index 50b6ff3340..c9ea15178d 100644
--- a/apps/controller/fw_att_control/ecl_controller.cpp
+++ b/apps/controller/fw_att_control/ecl_controller.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ecl_controller.cpp
@@ -52,68 +29,57 @@
 #include <mathlib/mathlib.h>
 
 ECL_Controller::ECL_Controller() :
-	_last_run(0),
-	_tc(0.1f),
-	_k_p(0.0f),
-	_k_i(0.0f),
-	_k_ff(0.0f),
-	_integrator_max(0.0f),
-	_max_rate(0.0f),
-	_last_output(0.0f),
-	_integrator(0.0f),
-	_euler_rate_setpoint(0.0f),
-	_body_rate_setpoint(0.0f)
-{
+    _last_run(0),
+    _tc(0.1f),
+    _k_p(0.0f),
+    _k_i(0.0f),
+    _k_ff(0.0f),
+    _integrator_max(0.0f),
+    _max_rate(0.0f),
+    _last_output(0.0f),
+    _integrator(0.0f),
+    _euler_rate_setpoint(0.0f),
+    _body_rate_setpoint(0.0f) {
 }
 
-void ECL_Controller::reset_integrator()
-{
-	_integrator = 0.0f;
+void ECL_Controller::reset_integrator() {
+    _integrator = 0.0f;
 }
 
-void ECL_Controller::set_time_constant(float time_constant)
-{
-	if (time_constant > 0.1f && time_constant < 3.0f) {
-		_tc = time_constant;
-	}
+void ECL_Controller::set_time_constant(float time_constant) {
+    if (time_constant > 0.1f && time_constant < 3.0f) {
+        _tc = time_constant;
+    }
 }
 
-void ECL_Controller::set_k_p(float k_p)
-{
-	_k_p = k_p;
+void ECL_Controller::set_k_p(float k_p) {
+    _k_p = k_p;
 }
 
-void ECL_Controller::set_k_i(float k_i)
-{
-	_k_i = k_i;
+void ECL_Controller::set_k_i(float k_i) {
+    _k_i = k_i;
 }
 
-void ECL_Controller::set_k_ff(float k_ff)
-{
-	_k_ff = k_ff;
+void ECL_Controller::set_k_ff(float k_ff) {
+    _k_ff = k_ff;
 }
 
-void ECL_Controller::set_integrator_max(float max)
-{
-	_integrator_max = max;
+void ECL_Controller::set_integrator_max(float max) {
+    _integrator_max = max;
 }
 
-void ECL_Controller::set_max_rate(float max_rate)
-{
-	_max_rate = max_rate;
+void ECL_Controller::set_max_rate(float max_rate) {
+    _max_rate = max_rate;
 }
 
-float ECL_Controller::get_euler_rate_setpoint()
-{
-	return _euler_rate_setpoint;
+float ECL_Controller::get_euler_rate_setpoint() {
+    return _euler_rate_setpoint;
 }
 
-float ECL_Controller::get_body_rate_setpoint()
-{
-	return _body_rate_setpoint;
+float ECL_Controller::get_body_rate_setpoint() {
+    return _body_rate_setpoint;
 }
 
-float ECL_Controller::get_integrator()
-{
-	return _integrator;
+float ECL_Controller::get_integrator() {
+    return _integrator;
 }
diff --git a/apps/controller/fw_att_control/ecl_pitch_controller.cpp b/apps/controller/fw_att_control/ecl_pitch_controller.cpp
index 7dd6591ac9..e2afff62ac 100644
--- a/apps/controller/fw_att_control/ecl_pitch_controller.cpp
+++ b/apps/controller/fw_att_control/ecl_pitch_controller.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ecl_pitch_controller.cpp
@@ -45,10 +22,7 @@
 
 float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data) {
     /* Do not calculate control signal with bad inputs */
-    if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
-          PX4_ISFINITE(ctl_data.roll) &&
-          PX4_ISFINITE(ctl_data.pitch) &&
-          PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) {
+    if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) {
         return _body_rate_setpoint;
     }
 
@@ -59,9 +33,8 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat
     _euler_rate_setpoint = pitch_error / _tc;
 
     /* Transform setpoint to body angular rates (jacobian) */
-    const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
-                                               cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
-    _body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
+    const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
+    _body_rate_setpoint                      = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
 
     return _body_rate_setpoint;
 }
diff --git a/apps/controller/fw_att_control/ecl_roll_controller.cpp b/apps/controller/fw_att_control/ecl_roll_controller.cpp
index 3c08244faf..c544bc52aa 100644
--- a/apps/controller/fw_att_control/ecl_roll_controller.cpp
+++ b/apps/controller/fw_att_control/ecl_roll_controller.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). 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 ECL 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ecl_roll_controller.cpp
@@ -45,10 +22,7 @@
 
 float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data) {
     /* Do not calculate control signal with bad inputs */
-    if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
-          PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
-          PX4_ISFINITE(ctl_data.pitch) &&
-          PX4_ISFINITE(ctl_data.roll))) {
+    if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.roll))) {
         return _body_rate_setpoint;
     }
 
@@ -59,9 +33,8 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData
     _euler_rate_setpoint = roll_error / _tc;
 
     /* Transform setpoint to body angular rates (jacobian) */
-    const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) *
-                                                                         ctl_data.euler_yaw_rate_setpoint;
-    _body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
+    const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.euler_yaw_rate_setpoint;
+    _body_rate_setpoint                     = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
 
     return _body_rate_setpoint;
 }
diff --git a/apps/controller/fw_att_control/ecl_wheel_controller.cpp b/apps/controller/fw_att_control/ecl_wheel_controller.cpp
index 9cb0c2442b..fe45a31716 100644
--- a/apps/controller/fw_att_control/ecl_wheel_controller.cpp
+++ b/apps/controller/fw_att_control/ecl_wheel_controller.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). 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 ECL 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ecl_wheel_controller.cpp
@@ -48,9 +25,7 @@ using matrix::wrap_pi;
 
 float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data) {
     /* Do not calculate control signal with bad inputs */
-    if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
-          PX4_ISFINITE(ctl_data.groundspeed) &&
-          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+    if (!(PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.groundspeed) && PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
         return math::constrain(_last_output, -1.0f, 1.0f);
     }
 
@@ -80,16 +55,14 @@ float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlDat
     }
 
     /* Apply PI rate controller and store non-limited output */
-    _last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
-                   ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
+    _last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
 
     return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data) {
     /* Do not calculate control signal with bad inputs */
-    if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
-          PX4_ISFINITE(ctl_data.yaw))) {
+    if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && PX4_ISFINITE(ctl_data.yaw))) {
         return _body_rate_setpoint;
     }
 
diff --git a/apps/estimator/sensors/data_validator/tests/test_data_validator.cpp b/apps/estimator/sensors/data_validator/tests/test_data_validator.cpp
index 56d1238b92..1bb592e272 100644
--- a/apps/estimator/sensors/data_validator/tests/test_data_validator.cpp
+++ b/apps/estimator/sensors/data_validator/tests/test_data_validator.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 /**
  * @file test_data_validator.cpp
  * Testing the DataValidator class
@@ -45,258 +22,250 @@
 #include <validation/data_validator.h>
 #include <validation/tests/tests_common.h>
 
+void test_init() {
+    printf("\n--- test_init ---\n");
 
-void test_init()
-{
-	printf("\n--- test_init ---\n");
-
-	uint64_t fake_timestamp = 666;
-	const uint32_t timeout_usec = 2000;//from original private value
+    uint64_t       fake_timestamp = 666;
+    const uint32_t timeout_usec   = 2000; //from original private value
 
-	DataValidator *validator = new DataValidator;
-	// initially there should be no siblings
-	assert(nullptr == validator->sibling());
-	// initially we should have zero confidence
-	assert(0.0f == validator->confidence(fake_timestamp));
-	// initially the error count should be zero
-	assert(0 == validator->error_count());
-	// initially unused
-	assert(!validator->used());
-	// initially no priority
-	assert(0 == validator->priority());
-	validator->set_timeout(timeout_usec);
-	assert(validator->get_timeout() == timeout_usec);
+    DataValidator *validator = new DataValidator;
+    // initially there should be no siblings
+    assert(nullptr == validator->sibling());
+    // initially we should have zero confidence
+    assert(0.0f == validator->confidence(fake_timestamp));
+    // initially the error count should be zero
+    assert(0 == validator->error_count());
+    // initially unused
+    assert(!validator->used());
+    // initially no priority
+    assert(0 == validator->priority());
+    validator->set_timeout(timeout_usec);
+    assert(validator->get_timeout() == timeout_usec);
 
 
-	DataValidator *sibling_validator = new DataValidator;
-	validator->setSibling(sibling_validator);
-	assert(sibling_validator == validator->sibling());
+    DataValidator *sibling_validator = new DataValidator;
+    validator->setSibling(sibling_validator);
+    assert(sibling_validator == validator->sibling());
 
-	//verify that with no data, confidence is zero and error mask is set
-	assert(0.0f == validator->confidence(fake_timestamp + 1));
-	uint32_t state = validator->state();
-	assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state));
+    //verify that with no data, confidence is zero and error mask is set
+    assert(0.0f == validator->confidence(fake_timestamp + 1));
+    uint32_t state = validator->state();
+    assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state));
 
-	//verify that calling print doesn't crash tests
-	validator->print();
+    //verify that calling print doesn't crash tests
+    validator->print();
 
-	delete validator; //force delete
+    delete validator; //force delete
 }
 
-void test_put()
-{
-	printf("\n--- test_put ---\n");
+void test_put() {
+    printf("\n--- test_put ---\n");
 
-	uint64_t timestamp = 500;
-	const uint32_t timeout_usec = 2000;//derived from class-private value
-	float val = 3.14159f;
-	//derived from class-private value: this is min change needed to avoid stale detection
-	const float sufficient_incr_value = (1.1f * 1E-6f);
+    uint64_t       timestamp    = 500;
+    const uint32_t timeout_usec = 2000; //derived from class-private value
+    float          val          = 3.14159f;
+    //derived from class-private value: this is min change needed to avoid stale detection
+    const float sufficient_incr_value = (1.1f * 1E-6f);
 
-	DataValidator *validator = new DataValidator;
-	fill_validator_with_samples(validator, sufficient_incr_value, &val, &timestamp);
+    DataValidator *validator = new DataValidator;
+    fill_validator_with_samples(validator, sufficient_incr_value, &val, &timestamp);
 
-	assert(validator->used());
-	//verify that the last value we inserted is the current validator value
-	float last_val = val - sufficient_incr_value;
-	assert(validator->value()[0] == last_val);
+    assert(validator->used());
+    //verify that the last value we inserted is the current validator value
+    float last_val = val - sufficient_incr_value;
+    assert(validator->value()[0] == last_val);
 
-	// we've just provided a bunch of valid data: should be fully confident
-	float conf = validator->confidence(timestamp);
+    // we've just provided a bunch of valid data: should be fully confident
+    float conf = validator->confidence(timestamp);
 
-	if (1.0f != conf) {
-		printf("conf: %f\n", (double)conf);
-		dump_validator_state(validator);
-	}
+    if (1.0f != conf) {
+        printf("conf: %f\n", (double)conf);
+        dump_validator_state(validator);
+    }
 
-	assert(1.0f == conf);
-	// should be no errors
-	assert(0 == validator->state());
+    assert(1.0f == conf);
+    // should be no errors
+    assert(0 == validator->state());
 
-	//now check confidence much beyond the timeout window-- should timeout
-	conf = validator->confidence(timestamp + (1.1 * timeout_usec));
+    //now check confidence much beyond the timeout window-- should timeout
+    conf = validator->confidence(timestamp + (1.1 * timeout_usec));
 
-	if (0.0f != conf) {
-		printf("conf: %f\n", (double)conf);
-		dump_validator_state(validator);
-	}
+    if (0.0f != conf) {
+        printf("conf: %f\n", (double)conf);
+        dump_validator_state(validator);
+    }
 
-	assert(0.0f == conf);
-	assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
+    assert(0.0f == conf);
+    assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
 
-	delete validator; //force delete
+    delete validator; //force delete
 }
 
 /**
  * Verify that the DataValidator detects sensor data that does not vary sufficiently
  */
-void test_stale_detector()
-{
-	printf("\n--- test_stale_detector ---\n");
+void test_stale_detector() {
+    printf("\n--- test_stale_detector ---\n");
 
-	uint64_t timestamp = 500;
-	float val = 3.14159f;
-	//derived from class-private value, this is insufficient to avoid stale detection:
-	const float insufficient_incr_value = (0.99 * 1E-6f);
+    uint64_t timestamp = 500;
+    float    val       = 3.14159f;
+    //derived from class-private value, this is insufficient to avoid stale detection:
+    const float insufficient_incr_value = (0.99 * 1E-6f);
 
-	DataValidator *validator = new DataValidator;
-	fill_validator_with_samples(validator, insufficient_incr_value, &val, &timestamp);
+    DataValidator *validator = new DataValidator;
+    fill_validator_with_samples(validator, insufficient_incr_value, &val, &timestamp);
 
-	// data is stale: should have no confidence
-	assert(0.0f == validator->confidence(timestamp));
+    // data is stale: should have no confidence
+    assert(0.0f == validator->confidence(timestamp));
 
-	// should be a stale error
-	uint32_t state = validator->state();
+    // should be a stale error
+    uint32_t state = validator->state();
 
-	if (DataValidator::ERROR_FLAG_STALE_DATA != state) {
-		dump_validator_state(validator);
-	}
+    if (DataValidator::ERROR_FLAG_STALE_DATA != state) {
+        dump_validator_state(validator);
+    }
 
-	assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state));
+    assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state));
 
-	delete validator; //force delete
+    delete validator; //force delete
 }
 
 /**
  * Verify the RMS error calculated by the DataValidator for a series of samples
  */
-void test_rms_calculation()
-{
-	printf("\n--- test_rms_calculation ---\n");
-	const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
-	const float mean_value = 3.14159f;
-	const uint32_t sample_count = 1000;
-	float expected_rms_err = 0.0f;
-	uint64_t timestamp = 500;
-
-	DataValidator *validator = new DataValidator;
-	validator->set_equal_value_threshold(equal_value_count);
-
-	insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, &timestamp);
-	float *rms = validator->rms();
-	assert(nullptr != rms);
-	float calc_rms_err = rms[0];
-	float diff = fabsf(calc_rms_err - expected_rms_err);
-	float diff_frac = (diff / expected_rms_err);
-	printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err,
-	       (double)diff, (double)diff_frac);
-	assert(diff_frac < 0.03f);
-
-	delete validator; //force delete
+void test_rms_calculation() {
+    printf("\n--- test_rms_calculation ---\n");
+    const int      equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
+    const float    mean_value        = 3.14159f;
+    const uint32_t sample_count      = 1000;
+    float          expected_rms_err  = 0.0f;
+    uint64_t       timestamp         = 500;
+
+    DataValidator *validator = new DataValidator;
+    validator->set_equal_value_threshold(equal_value_count);
+
+    insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, &timestamp);
+    float *rms = validator->rms();
+    assert(nullptr != rms);
+    float calc_rms_err = rms[0];
+    float diff         = fabsf(calc_rms_err - expected_rms_err);
+    float diff_frac    = (diff / expected_rms_err);
+    printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err,
+           (double)diff, (double)diff_frac);
+    assert(diff_frac < 0.03f);
+
+    delete validator; //force delete
 }
 
 /**
  * Verify error tracking performed by DataValidator::put
  */
-void test_error_tracking()
-{
-	printf("\n--- test_error_tracking ---\n");
-	uint64_t timestamp = 500;
-	uint64_t timestamp_incr = 5;
-	const uint32_t timeout_usec = 2000;//from original private value
-	float val = 3.14159f;
-	uint32_t error_count = 0;
-	int expected_error_density = 0;
-	uint8_t priority = 50;
-	//from private value: this is min change needed to avoid stale detection
-	const float sufficient_incr_value = (1.1f * 1E-6f);
-	//default is private VALUE_EQUAL_COUNT_DEFAULT
-	const int equal_value_count = 50000;
-	//should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT
-	const int total_iterations = 1000;
-
-	DataValidator *validator = new DataValidator;
-	validator->set_timeout(timeout_usec);
-	validator->set_equal_value_threshold(equal_value_count);
-
-	//put a bunch of values that are all different
-	for (int i = 0; i < total_iterations;  i++, val += sufficient_incr_value) {
-		timestamp += timestamp_incr;
-
-		//up to a 50% random error rate appears to pass the error density filter
-		if ((((float)rand() / (float)RAND_MAX)) < 0.500f) {
-			error_count += 1;
-			expected_error_density += 1;
-
-		} else if (expected_error_density > 0) {
-			expected_error_density -= 1;
-		}
-
-		validator->put(timestamp, val, error_count, priority);
-	}
-
-	assert(validator->used());
-	//at this point, error_count should be less than NORETURN_ERRCOUNT
-	assert(validator->error_count() == error_count);
-
-	// we've just provided a bunch of valid data with some errors:
-	// confidence should be reduced by the number of errors
-	float conf = validator->confidence(timestamp);
-	printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf);
-	assert(1.0f != conf);  //we should not be fully confident
-	assert(0.0f != conf);  //neither should we be completely unconfident
-	// should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT
-	assert(0 == validator->state());
-
-	// the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW)
-	// ERROR_DENSITY_WINDOW is currently private, but == 100.0f
-	float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f);
-	double diff = fabs(reduced_conf - conf);
-
-	if (reduced_conf != conf) {
-		printf("conf: %f reduced_conf: %f diff: %f\n",
-		       (double)conf, (double)reduced_conf, diff);
-		dump_validator_state(validator);
-	}
-
-	assert(diff < 1E-6f);
-
-	//Now, insert a series of errors and ensure we trip the error detector
-	for (int i = 0; i < 250;  i++, val += sufficient_incr_value) {
-		timestamp += timestamp_incr;
-		//100% error rate
-		error_count += 1;
-		expected_error_density += 1;
-		validator->put(timestamp, val, error_count, priority);
-	}
-
-	conf = validator->confidence(timestamp);
-	assert(0.0f == conf);  // should we be completely unconfident
-	// we should have triggered the high error density detector
-	assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state()));
-
-
-	validator->reset_state();
-
-	//Now insert so many errors that we exceed private NORETURN_ERRCOUNT
-	for (int i = 0; i < 10000;  i++, val += sufficient_incr_value) {
-		timestamp += timestamp_incr;
-		//100% error rate
-		error_count += 1;
-		expected_error_density += 1;
-		validator->put(timestamp, val, error_count, priority);
-	}
-
-	conf = validator->confidence(timestamp);
-	assert(0.0f == conf);  // should we be completely unconfident
-	// we should have triggered the high error count detector
-	assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state()));
-
-	delete validator; //force delete
-
+void test_error_tracking() {
+    printf("\n--- test_error_tracking ---\n");
+    uint64_t       timestamp              = 500;
+    uint64_t       timestamp_incr         = 5;
+    const uint32_t timeout_usec           = 2000; //from original private value
+    float          val                    = 3.14159f;
+    uint32_t       error_count            = 0;
+    int            expected_error_density = 0;
+    uint8_t        priority               = 50;
+    //from private value: this is min change needed to avoid stale detection
+    const float sufficient_incr_value = (1.1f * 1E-6f);
+    //default is private VALUE_EQUAL_COUNT_DEFAULT
+    const int equal_value_count = 50000;
+    //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT
+    const int total_iterations = 1000;
+
+    DataValidator *validator = new DataValidator;
+    validator->set_timeout(timeout_usec);
+    validator->set_equal_value_threshold(equal_value_count);
+
+    //put a bunch of values that are all different
+    for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) {
+        timestamp += timestamp_incr;
+
+        //up to a 50% random error rate appears to pass the error density filter
+        if ((((float)rand() / (float)RAND_MAX)) < 0.500f) {
+            error_count            += 1;
+            expected_error_density += 1;
+
+        } else if (expected_error_density > 0) {
+            expected_error_density -= 1;
+        }
+
+        validator->put(timestamp, val, error_count, priority);
+    }
+
+    assert(validator->used());
+    //at this point, error_count should be less than NORETURN_ERRCOUNT
+    assert(validator->error_count() == error_count);
+
+    // we've just provided a bunch of valid data with some errors:
+    // confidence should be reduced by the number of errors
+    float conf = validator->confidence(timestamp);
+    printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf);
+    assert(1.0f != conf); //we should not be fully confident
+    assert(0.0f != conf); //neither should we be completely unconfident
+    // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT
+    assert(0 == validator->state());
+
+    // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW)
+    // ERROR_DENSITY_WINDOW is currently private, but == 100.0f
+    float  reduced_conf = 1.0f - ((float)expected_error_density / 100.0f);
+    double diff         = fabs(reduced_conf - conf);
+
+    if (reduced_conf != conf) {
+        printf("conf: %f reduced_conf: %f diff: %f\n",
+               (double)conf, (double)reduced_conf, diff);
+        dump_validator_state(validator);
+    }
+
+    assert(diff < 1E-6f);
+
+    //Now, insert a series of errors and ensure we trip the error detector
+    for (int i = 0; i < 250; i++, val += sufficient_incr_value) {
+        timestamp += timestamp_incr;
+        //100% error rate
+        error_count            += 1;
+        expected_error_density += 1;
+        validator->put(timestamp, val, error_count, priority);
+    }
+
+    conf = validator->confidence(timestamp);
+    assert(0.0f == conf); // should we be completely unconfident
+    // we should have triggered the high error density detector
+    assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state()));
+
+
+    validator->reset_state();
+
+    //Now insert so many errors that we exceed private NORETURN_ERRCOUNT
+    for (int i = 0; i < 10000; i++, val += sufficient_incr_value) {
+        timestamp += timestamp_incr;
+        //100% error rate
+        error_count            += 1;
+        expected_error_density += 1;
+        validator->put(timestamp, val, error_count, priority);
+    }
+
+    conf = validator->confidence(timestamp);
+    assert(0.0f == conf); // should we be completely unconfident
+    // we should have triggered the high error count detector
+    assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state()));
+
+    delete validator; //force delete
 }
 
-int main(int argc, char *argv[])
-{
-	(void)argc; // unused
-	(void)argv; // unused
+int main(int argc, char *argv[]) {
+    (void)argc; // unused
+    (void)argv; // unused
 
-	srand(666);
-	test_init();
-	test_put();
-	test_stale_detector();
-	test_rms_calculation();
-	test_error_tracking();
+    srand(666);
+    test_init();
+    test_put();
+    test_stale_detector();
+    test_rms_calculation();
+    test_error_tracking();
 
-	return 0; //passed
+    return 0; //passed
 }
diff --git a/apps/estimator/sensors/data_validator/tests/test_data_validator_group.cpp b/apps/estimator/sensors/data_validator/tests/test_data_validator_group.cpp
index 2af5c2acd4..c5ed0a96a9 100644
--- a/apps/estimator/sensors/data_validator/tests/test_data_validator_group.cpp
+++ b/apps/estimator/sensors/data_validator/tests/test_data_validator_group.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 /**
  * @file test_data_validator_group.cpp
  * Testing the DataValidatorGroup class
@@ -47,41 +24,39 @@
 #include <validation/tests/tests_common.h>
 
 
-const uint32_t base_timeout_usec = 2000;//from original private value
-const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
-const uint64_t base_timestamp = 666;
+const uint32_t base_timeout_usec = 2000; //from original private value
+const int      equal_value_count = 100;  //default is private VALUE_EQUAL_COUNT_DEFAULT
+const uint64_t base_timestamp    = 666;
 const unsigned base_num_siblings = 4;
 
-
 /**
  * Initialize a DataValidatorGroup with some common settings;
  * @param sibling_count (out) the number of siblings initialized
  */
-DataValidatorGroup  *setup_base_group(unsigned *sibling_count)
-{
-	unsigned num_siblings = base_num_siblings;
-
-	DataValidatorGroup *group = new DataValidatorGroup(num_siblings);
-	assert(nullptr != group);
-	//verify that calling print doesn't crash the tests
-	group->print();
-	printf("\n");
-
-	//should be no failovers yet
-	assert(0 == group->failover_count());
-	assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
-	assert(-1 == group->failover_index());
-
-	//this sets the timeout on all current members of the group, as well as members added later
-	group->set_timeout(base_timeout_usec);
-	//the following sets the threshold on all CURRENT members of the group, but not any added later
-	//TODO This is likely a bug in DataValidatorGroup
-	group->set_equal_value_threshold(equal_value_count);
-
-	//return values
-	*sibling_count = num_siblings;
-
-	return group;
+DataValidatorGroup *setup_base_group(unsigned *sibling_count) {
+    unsigned num_siblings = base_num_siblings;
+
+    DataValidatorGroup *group = new DataValidatorGroup(num_siblings);
+    assert(nullptr != group);
+    //verify that calling print doesn't crash the tests
+    group->print();
+    printf("\n");
+
+    //should be no failovers yet
+    assert(0 == group->failover_count());
+    assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
+    assert(-1 == group->failover_index());
+
+    //this sets the timeout on all current members of the group, as well as members added later
+    group->set_timeout(base_timeout_usec);
+    //the following sets the threshold on all CURRENT members of the group, but not any added later
+    //TODO This is likely a bug in DataValidatorGroup
+    group->set_equal_value_threshold(equal_value_count);
+
+    //return values
+    *sibling_count = num_siblings;
+
+    return group;
 }
 
 /**
@@ -91,27 +66,24 @@ DataValidatorGroup  *setup_base_group(unsigned *sibling_count)
  * @param val1_idx Index of the validator to fill with samples
  * @param num_samples
  */
-void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx,  uint32_t num_samples)
-{
-	uint64_t timestamp = base_timestamp;
-	uint32_t error_count = 0;
-	float last_best_val = 0.0f;
-
-	for (uint32_t i = 0; i < num_samples; i++) {
-		float val = ((float) rand() / (float) RAND_MAX);
-		float data[DataValidator::dimensions] = {val};
-		group->put(val1_idx, timestamp, data, error_count, 100);
-		last_best_val = val;
-	}
-
-	int best_idx = 0;
-	float *best_data = group->get_best(timestamp, &best_idx);
-	assert(last_best_val == best_data[0]);
-	assert(best_idx == val1_idx);
+void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples) {
+    uint64_t timestamp     = base_timestamp;
+    uint32_t error_count   = 0;
+    float    last_best_val = 0.0f;
+
+    for (uint32_t i = 0; i < num_samples; i++) {
+        float val                             = ((float)rand() / (float)RAND_MAX);
+        float data[DataValidator::dimensions] = {val};
+        group->put(val1_idx, timestamp, data, error_count, 100);
+        last_best_val = val;
+    }
+
+    int    best_idx  = 0;
+    float *best_data = group->get_best(timestamp, &best_idx);
+    assert(last_best_val == best_data[0]);
+    assert(best_idx == val1_idx);
 }
 
-
-
 /**
  * Fill two validators in the group with samples, by index.
  * Both validators will be filled with the same data, but
@@ -122,26 +94,24 @@ void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx,  uint32_t
  * @param val2_idx index of the second validator to fill
  * @param num_samples
  */
-void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples)
-{
-	uint64_t timestamp = base_timestamp;
-	uint32_t error_count = 0;
-	float last_best_val = 0.0f;
-
-	for (uint32_t i = 0; i < num_samples; i++) {
-		float val = ((float) rand() / (float) RAND_MAX);
-		float data[DataValidator::dimensions] = {val};
-		//two sensors with identical values, but different priorities
-		group->put(val1_idx, timestamp, data, error_count, 100);
-		group->put(val2_idx, timestamp, data, error_count, 10);
-		last_best_val = val;
-	}
-
-	int best_idx = 0;
-	float *best_data = group->get_best(timestamp, &best_idx);
-	assert(last_best_val == best_data[0]);
-	assert(best_idx == val1_idx);
-
+void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples) {
+    uint64_t timestamp     = base_timestamp;
+    uint32_t error_count   = 0;
+    float    last_best_val = 0.0f;
+
+    for (uint32_t i = 0; i < num_samples; i++) {
+        float val                             = ((float)rand() / (float)RAND_MAX);
+        float data[DataValidator::dimensions] = {val};
+        //two sensors with identical values, but different priorities
+        group->put(val1_idx, timestamp, data, error_count, 100);
+        group->put(val2_idx, timestamp, data, error_count, 10);
+        last_best_val = val;
+    }
+
+    int    best_idx  = 0;
+    float *best_data = group->get_best(timestamp, &best_idx);
+    assert(last_best_val == best_data[0]);
+    assert(best_idx == val1_idx);
 }
 
 /**
@@ -149,16 +119,15 @@ void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_
  * @param group
  * @return
  */
-DataValidator *add_validator_to_group(DataValidatorGroup *group)
-{
-	DataValidator *validator = group->add_new_validator();
-	//verify the previously set timeout applies to the new group member
-	assert(validator->get_timeout() == base_timeout_usec);
-	//for testing purposes, ensure this newly added member is consistent with the rest of the group
-	//TODO this is likely a bug in DataValidatorGroup
-	validator->set_equal_value_threshold(equal_value_count);
-
-	return validator;
+DataValidator *add_validator_to_group(DataValidatorGroup *group) {
+    DataValidator *validator = group->add_new_validator();
+    //verify the previously set timeout applies to the new group member
+    assert(validator->get_timeout() == base_timeout_usec);
+    //for testing purposes, ensure this newly added member is consistent with the rest of the group
+    //TODO this is likely a bug in DataValidatorGroup
+    validator->set_equal_value_threshold(equal_value_count);
+
+    return validator;
 }
 
 /**
@@ -170,216 +139,206 @@ DataValidator *add_validator_to_group(DataValidatorGroup *group)
  * @return
  */
 DataValidatorGroup *setup_group_with_two_validator_handles(
-	DataValidator **validator1_handle,
-	DataValidator **validator2_handle,
-	unsigned *sibling_count)
-{
-	DataValidatorGroup *group = setup_base_group(sibling_count);
-
-	//now we add validators
-	*validator1_handle = add_validator_to_group(group);
-	*validator2_handle = add_validator_to_group(group);
-	*sibling_count += 2;
-
-	return group;
-}
+    DataValidator **validator1_handle,
+    DataValidator **validator2_handle,
+    unsigned       *sibling_count) {
+    DataValidatorGroup *group = setup_base_group(sibling_count);
+
+    //now we add validators
+    *validator1_handle  = add_validator_to_group(group);
+    *validator2_handle  = add_validator_to_group(group);
+    *sibling_count     += 2;
 
+    return group;
+}
 
-void test_init()
-{
-	unsigned num_siblings = 0;
+void test_init() {
+    unsigned num_siblings = 0;
 
-	DataValidatorGroup *group = setup_base_group(&num_siblings);
+    DataValidatorGroup *group = setup_base_group(&num_siblings);
 
-	//should not yet be any best value
-	int best_index = -1;
-	assert(nullptr == group->get_best(base_timestamp, &best_index));
+    //should not yet be any best value
+    int best_index = -1;
+    assert(nullptr == group->get_best(base_timestamp, &best_index));
 
-	delete group; //force cleanup
+    delete group; //force cleanup
 }
 
-
 /**
  * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority
  */
-void test_put()
-{
-	unsigned num_siblings = 0;
-	DataValidator *validator1 = nullptr;
-	DataValidator *validator2 = nullptr;
-
-	uint64_t timestamp = base_timestamp;
-
-	DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
-	printf("num_siblings: %d \n", num_siblings);
-	unsigned val1_idx = num_siblings - 2;
-	unsigned val2_idx = num_siblings - 1;
-
-	fill_two_with_valid_data(group, val1_idx, val2_idx, 500);
-	int best_idx = -1;
-	float *best_data = group->get_best(timestamp, &best_idx);
-	assert(nullptr != best_data);
-	float best_val = best_data[0];
-
-	float *cur_val1 = validator1->value();
-	assert(nullptr != cur_val1);
-	//printf("cur_val1 %p \n", cur_val1);
-	assert(best_val == cur_val1[0]);
-
-	float *cur_val2 = validator2->value();
-	assert(nullptr != cur_val2);
-	//printf("cur_val12 %p \n", cur_val2);
-	assert(best_val == cur_val2[0]);
-
-	delete group; //force cleanup
+void test_put() {
+    unsigned       num_siblings = 0;
+    DataValidator *validator1   = nullptr;
+    DataValidator *validator2   = nullptr;
+
+    uint64_t timestamp = base_timestamp;
+
+    DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
+    printf("num_siblings: %d \n", num_siblings);
+    unsigned val1_idx = num_siblings - 2;
+    unsigned val2_idx = num_siblings - 1;
+
+    fill_two_with_valid_data(group, val1_idx, val2_idx, 500);
+    int    best_idx  = -1;
+    float *best_data = group->get_best(timestamp, &best_idx);
+    assert(nullptr != best_data);
+    float best_val = best_data[0];
+
+    float *cur_val1 = validator1->value();
+    assert(nullptr != cur_val1);
+    //printf("cur_val1 %p \n", cur_val1);
+    assert(best_val == cur_val1[0]);
+
+    float *cur_val2 = validator2->value();
+    assert(nullptr != cur_val2);
+    //printf("cur_val12 %p \n", cur_val2);
+    assert(best_val == cur_val2[0]);
+
+    delete group; //force cleanup
 }
 
-
 /**
  * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best".
  */
-void test_priority_switch()
-{
-	unsigned num_siblings = 0;
-	DataValidator *validator1 = nullptr;
-	DataValidator *validator2 = nullptr;
-
-	uint64_t timestamp = base_timestamp;
-
-	DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
-	//printf("num_siblings: %d \n",num_siblings);
-	int val1_idx = (int)num_siblings - 2;
-	int val2_idx = (int)num_siblings - 1;
-	uint32_t error_count = 0;
-
-	fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
-
-	int best_idx = -1;
-	float *best_data = nullptr;
-	//now, switch the priorities, which switches "best" but doesn't trigger a failover
-	float new_best_val = 3.14159f;
-	float data[DataValidator::dimensions] = {new_best_val};
-	//a single sample insertion should be sufficient to trigger a priority switch
-	group->put(val1_idx, timestamp, data, error_count, 1);
-	group->put(val2_idx, timestamp, data, error_count, 100);
-	best_data = group->get_best(timestamp, &best_idx);
-	assert(new_best_val == best_data[0]);
-	//the new best sensor should now be the sensor with the higher priority
-	assert(best_idx == val2_idx);
-	//should not have detected a real failover
-	assert(0 == group->failover_count());
-
-	delete  group; //cleanup
+void test_priority_switch() {
+    unsigned       num_siblings = 0;
+    DataValidator *validator1   = nullptr;
+    DataValidator *validator2   = nullptr;
+
+    uint64_t timestamp = base_timestamp;
+
+    DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
+    //printf("num_siblings: %d \n",num_siblings);
+    int      val1_idx    = (int)num_siblings - 2;
+    int      val2_idx    = (int)num_siblings - 1;
+    uint32_t error_count = 0;
+
+    fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
+
+    int    best_idx  = -1;
+    float *best_data = nullptr;
+    //now, switch the priorities, which switches "best" but doesn't trigger a failover
+    float new_best_val                    = 3.14159f;
+    float data[DataValidator::dimensions] = {new_best_val};
+    //a single sample insertion should be sufficient to trigger a priority switch
+    group->put(val1_idx, timestamp, data, error_count, 1);
+    group->put(val2_idx, timestamp, data, error_count, 100);
+    best_data = group->get_best(timestamp, &best_idx);
+    assert(new_best_val == best_data[0]);
+    //the new best sensor should now be the sensor with the higher priority
+    assert(best_idx == val2_idx);
+    //should not have detected a real failover
+    assert(0 == group->failover_count());
+
+    delete group; //cleanup
 }
 
 /**
  * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors
  */
-void test_simple_failover()
-{
-	unsigned num_siblings = 0;
-	DataValidator *validator1 = nullptr;
-	DataValidator *validator2 = nullptr;
+void test_simple_failover() {
+    unsigned       num_siblings = 0;
+    DataValidator *validator1   = nullptr;
+    DataValidator *validator2   = nullptr;
 
-	uint64_t timestamp = base_timestamp;
+    uint64_t timestamp = base_timestamp;
 
-	DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
-	//printf("num_siblings: %d \n",num_siblings);
-	int val1_idx = (int)num_siblings - 2;
-	int val2_idx = (int)num_siblings - 1;
+    DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
+    //printf("num_siblings: %d \n",num_siblings);
+    int val1_idx = (int)num_siblings - 2;
+    int val2_idx = (int)num_siblings - 1;
 
 
-	fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
+    fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
 
-	int best_idx = -1;
-	float *best_data = nullptr;
+    int    best_idx  = -1;
+    float *best_data = nullptr;
 
-	//trigger a real failover
-	float new_best_val = 3.14159f;
-	float data[DataValidator::dimensions] = {new_best_val};
-	//trigger a bunch of errors on the previous best sensor
-	unsigned val1_err_count = 0;
+    //trigger a real failover
+    float new_best_val                    = 3.14159f;
+    float data[DataValidator::dimensions] = {new_best_val};
+    //trigger a bunch of errors on the previous best sensor
+    unsigned val1_err_count = 0;
 
-	for (int i = 0; i < 25; i++) {
-		group->put(val1_idx, timestamp, data, ++val1_err_count, 100);
-		group->put(val2_idx, timestamp, data, 0, 10);
-	}
+    for (int i = 0; i < 25; i++) {
+        group->put(val1_idx, timestamp, data, ++val1_err_count, 100);
+        group->put(val2_idx, timestamp, data, 0, 10);
+    }
 
-	assert(validator1->error_count() == val1_err_count);
+    assert(validator1->error_count() == val1_err_count);
 
-	//since validator1 is experiencing errors, we should see a failover to validator2
-	best_data = group->get_best(timestamp + 1, &best_idx);
-	assert(nullptr != best_data);
-	assert(new_best_val == best_data[0]);
-	assert(best_idx == val2_idx);
-	//should have detected a real failover
-	printf("failover_count: %d \n", group->failover_count());
-	assert(1 == group->failover_count());
+    //since validator1 is experiencing errors, we should see a failover to validator2
+    best_data = group->get_best(timestamp + 1, &best_idx);
+    assert(nullptr != best_data);
+    assert(new_best_val == best_data[0]);
+    assert(best_idx == val2_idx);
+    //should have detected a real failover
+    printf("failover_count: %d \n", group->failover_count());
+    assert(1 == group->failover_count());
 
-	//even though validator1 has encountered a bunch of errors, it hasn't failed
-	assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state());
+    //even though validator1 has encountered a bunch of errors, it hasn't failed
+    assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state());
 
-	// although we failed over from one sensor to another, this is not the same thing tracked by failover_index
-	int fail_idx = group->failover_index();
-	assert(-1 == fail_idx);//no failed sensor
+    // although we failed over from one sensor to another, this is not the same thing tracked by failover_index
+    int fail_idx = group->failover_index();
+    assert(-1 == fail_idx); //no failed sensor
 
-	//since no sensor has actually hard-failed, the group failover state is NO_ERROR
-	assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
+    //since no sensor has actually hard-failed, the group failover state is NO_ERROR
+    assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state());
 
 
-	delete  group; //cleanup
+    delete group; //cleanup
 }
 
 /**
  * Force once sensor to fail and ensure that we detect it
  */
-void test_sensor_failure()
-{
-	unsigned num_siblings = 0;
-	uint64_t timestamp = base_timestamp;
-	const float sufficient_incr_value = (1.1f * 1E-6f);
-	const uint32_t timeout_usec = 2000;//derived from class-private value
+void test_sensor_failure() {
+    unsigned       num_siblings          = 0;
+    uint64_t       timestamp             = base_timestamp;
+    const float    sufficient_incr_value = (1.1f * 1E-6f);
+    const uint32_t timeout_usec          = 2000; //derived from class-private value
 
-	float val = 3.14159f;
+    float val = 3.14159f;
 
-	DataValidatorGroup *group =  setup_base_group(&num_siblings);
+    DataValidatorGroup *group = setup_base_group(&num_siblings);
 
-	//now we add validators
-	DataValidator *validator  = add_validator_to_group(group);
-	assert(nullptr != validator);
-	num_siblings++;
-	int val_idx = num_siblings - 1;
+    //now we add validators
+    DataValidator *validator = add_validator_to_group(group);
+    assert(nullptr != validator);
+    num_siblings++;
+    int val_idx = num_siblings - 1;
 
-	fill_validator_with_samples(validator,  sufficient_incr_value, &val, &timestamp);
-	//the best should now be the one validator we've filled with samples
+    fill_validator_with_samples(validator, sufficient_incr_value, &val, &timestamp);
+    //the best should now be the one validator we've filled with samples
 
-	int best_idx = -1;
-	float *best_data = group->get_best(timestamp, &best_idx);
-	assert(nullptr != best_data);
-	//printf("best_idx: %d val_idx: %d\n", best_idx, val_idx);
-	assert(best_idx == val_idx);
+    int    best_idx  = -1;
+    float *best_data = group->get_best(timestamp, &best_idx);
+    assert(nullptr != best_data);
+    //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx);
+    assert(best_idx == val_idx);
 
-	//now force a timeout failure in the one validator, by checking confidence long past timeout
-	validator->confidence(timestamp + (1.1 * timeout_usec));
-	assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
+    //now force a timeout failure in the one validator, by checking confidence long past timeout
+    validator->confidence(timestamp + (1.1 * timeout_usec));
+    assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state()));
 
-	//now that the one sensor has failed, the group should detect this as well
-	int fail_idx = group->failover_index();
-	assert(val_idx == fail_idx);
+    //now that the one sensor has failed, the group should detect this as well
+    int fail_idx = group->failover_index();
+    assert(val_idx == fail_idx);
 
-	delete  group;
+    delete group;
 }
 
-int main(int argc, char *argv[])
-{
-	(void)argc; // unused
-	(void)argv; // unused
+int main(int argc, char *argv[]) {
+    (void)argc; // unused
+    (void)argv; // unused
 
-	test_init();
-	test_put();
-	test_simple_failover();
-	test_priority_switch();
-	test_sensor_failure();
+    test_init();
+    test_put();
+    test_simple_failover();
+    test_priority_switch();
+    test_sensor_failure();
 
-	return 0; //passed
+    return 0; //passed
 }
diff --git a/apps/estimator/sensors/data_validator/tests/tests_common.cpp b/apps/estimator/sensors/data_validator/tests/tests_common.cpp
index 899f7871f3..cbdfb5022b 100644
--- a/apps/estimator/sensors/data_validator/tests/tests_common.cpp
+++ b/apps/estimator/sensors/data_validator/tests/tests_common.cpp
@@ -1,103 +1,74 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <stdio.h>
 #include "tests_common.h"
 
-
 void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err,
-			       uint64_t *timestamp_io)
-{
-	uint64_t timestamp = *timestamp_io;
-	uint64_t timestamp_incr = 5;
-	const uint32_t error_count = 0;
-	const uint8_t priority = 50;
-	const float swing = 1E-2f;
-	double sum_dev_squares = 0.0f;
+                               uint64_t *timestamp_io) {
+    uint64_t       timestamp       = *timestamp_io;
+    uint64_t       timestamp_incr  = 5;
+    const uint32_t error_count     = 0;
+    const uint8_t  priority        = 50;
+    const float    swing           = 1E-2f;
+    double         sum_dev_squares = 0.0f;
 
-	//insert a series of values that swing around the mean
-	for (uint32_t i = 0; i < count;  i++) {
-		float iter_swing = (0 == (i % 2)) ? swing : -swing;
-		float iter_val = mean + iter_swing;
-		float iter_dev = iter_val - mean;
-		sum_dev_squares += (iter_dev * iter_dev);
-		timestamp += timestamp_incr;
-		validator->put(timestamp, iter_val, error_count, priority);
-	}
+    //insert a series of values that swing around the mean
+    for (uint32_t i = 0; i < count; i++) {
+        float iter_swing  = (0 == (i % 2)) ? swing : -swing;
+        float iter_val    = mean + iter_swing;
+        float iter_dev    = iter_val - mean;
+        sum_dev_squares  += (iter_dev * iter_dev);
+        timestamp        += timestamp_incr;
+        validator->put(timestamp, iter_val, error_count, priority);
+    }
 
-	double rms = sqrt(sum_dev_squares / (double)count);
-	//note: this should be approximately equal to "swing"
-	*rms_err = (float)rms;
-	*timestamp_io = timestamp;
+    double rms = sqrt(sum_dev_squares / (double)count);
+    //note: this should be approximately equal to "swing"
+    *rms_err      = (float)rms;
+    *timestamp_io = timestamp;
 }
 
-void dump_validator_state(DataValidator *validator)
-{
-	uint32_t state = validator->state();
-	printf("state: 0x%x no_data: %d stale: %d timeout:%d\n",
-	       validator->state(),
-	       DataValidator::ERROR_FLAG_NO_DATA & state,
-	       DataValidator::ERROR_FLAG_STALE_DATA & state,
-	       DataValidator::ERROR_FLAG_TIMEOUT & state
-	      );
-	validator->print();
-	printf("\n");
+void dump_validator_state(DataValidator *validator) {
+    uint32_t state = validator->state();
+    printf("state: 0x%x no_data: %d stale: %d timeout:%d\n",
+           validator->state(),
+           DataValidator::ERROR_FLAG_NO_DATA & state,
+           DataValidator::ERROR_FLAG_STALE_DATA & state,
+           DataValidator::ERROR_FLAG_TIMEOUT & state);
+    validator->print();
+    printf("\n");
 }
 
 void fill_validator_with_samples(DataValidator *validator,
-				 const float incr_value,
-				 float *value_io,
-				 uint64_t *timestamp_io)
-{
-	uint64_t timestamp = *timestamp_io;
-	const uint64_t timestamp_incr = 5; //usec
-	const uint32_t timeout_usec = 2000;//derived from class-private value
-
-	float val = *value_io;
-	const uint32_t error_count = 0;
-	const uint8_t priority = 50; //"medium" priority
-	const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
+                                 const float    incr_value,
+                                 float         *value_io,
+                                 uint64_t      *timestamp_io) {
+    uint64_t       timestamp      = *timestamp_io;
+    const uint64_t timestamp_incr = 5;    //usec
+    const uint32_t timeout_usec   = 2000; //derived from class-private value
 
-	validator->set_equal_value_threshold(equal_value_count);
-	validator->set_timeout(timeout_usec);
+    float          val               = *value_io;
+    const uint32_t error_count       = 0;
+    const uint8_t  priority          = 50;  //"medium" priority
+    const int      equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
 
-	//put a bunch of values that are all different
-	for (int i = 0; i < equal_value_count; i++, val += incr_value) {
-		timestamp += timestamp_incr;
-		validator->put(timestamp, val, error_count, priority);
-	}
+    validator->set_equal_value_threshold(equal_value_count);
+    validator->set_timeout(timeout_usec);
 
-	*timestamp_io = timestamp;
-	*value_io = val;
+    //put a bunch of values that are all different
+    for (int i = 0; i < equal_value_count; i++, val += incr_value) {
+        timestamp += timestamp_incr;
+        validator->put(timestamp, val, error_count, priority);
+    }
 
+    *timestamp_io = timestamp;
+    *value_io     = val;
 }
diff --git a/apps/estimator/sensors/data_validator/tests/tests_common.h b/apps/estimator/sensors/data_validator/tests/tests_common.h
index ae1351dfe1..19712832c1 100644
--- a/apps/estimator/sensors/data_validator/tests/tests_common.h
+++ b/apps/estimator/sensors/data_validator/tests/tests_common.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifndef ECL_TESTS_COMMON_H
 #define ECL_TESTS_COMMON_H
@@ -45,7 +22,7 @@
  * @param timestamp_io (in/out) in: start timestamp, out: last timestamp
  */
 void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err,
-			       uint64_t *timestamp_io);
+                               uint64_t *timestamp_io);
 
 /**
  * Print out the state of a DataValidator
@@ -61,8 +38,8 @@ void dump_validator_state(DataValidator *validator);
  * @param timestamp_io (in/out) in: initial timestamp, out: final timestamp
  */
 void fill_validator_with_samples(DataValidator *validator,
-				 const float incr_value,
-				 float *value_io,
-				 uint64_t *timestamp_io);
+                                 const float    incr_value,
+                                 float         *value_io,
+                                 uint64_t      *timestamp_io);
 
 #endif //ECL_TESTS_COMMON_H
diff --git a/apps/libraries/controller/terrain_estimation/terrain_estimator.cpp b/apps/libraries/controller/terrain_estimation/terrain_estimator.cpp
index f6e7e52d97..a42d14fa8d 100644
--- a/apps/libraries/controller/terrain_estimation/terrain_estimator.cpp
+++ b/apps/libraries/controller/terrain_estimation/terrain_estimator.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015 Roman Bapst. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file terrain_estimator.cpp
diff --git a/apps/libraries/controller/terrain_estimation/terrain_estimator.h b/apps/libraries/controller/terrain_estimation/terrain_estimator.h
index 243197868f..e4f539f43d 100644
--- a/apps/libraries/controller/terrain_estimation/terrain_estimator.h
+++ b/apps/libraries/controller/terrain_estimation/terrain_estimator.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015 Roman Bapst. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file terrain_estimator.h
diff --git a/apps/libraries/estimator/terrain_estimation/terrain_estimator.cpp b/apps/libraries/estimator/terrain_estimation/terrain_estimator.cpp
index f6e7e52d97..a42d14fa8d 100644
--- a/apps/libraries/estimator/terrain_estimation/terrain_estimator.cpp
+++ b/apps/libraries/estimator/terrain_estimation/terrain_estimator.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015 Roman Bapst. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file terrain_estimator.cpp
diff --git a/apps/libraries/estimator/terrain_estimation/terrain_estimator.h b/apps/libraries/estimator/terrain_estimation/terrain_estimator.h
index 243197868f..e4f539f43d 100644
--- a/apps/libraries/estimator/terrain_estimation/terrain_estimator.h
+++ b/apps/libraries/estimator/terrain_estimation/terrain_estimator.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015 Roman Bapst. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file terrain_estimator.h
diff --git a/apps/libraries/estimator/world_magnetic_model/geo_magnetic_tables.hpp b/apps/libraries/estimator/world_magnetic_model/geo_magnetic_tables.hpp
index 3366c1d4bf..6f2d0bf365 100644
--- a/apps/libraries/estimator/world_magnetic_model/geo_magnetic_tables.hpp
+++ b/apps/libraries/estimator/world_magnetic_model/geo_magnetic_tables.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <stdint.h>
 
diff --git a/apps/peripheral/actuator/pwm_out/CMakeLists.txt b/apps/peripheral/actuator/pwm_out/CMakeLists.txt
deleted file mode 100644
index b102064b19..0000000000
--- a/apps/peripheral/actuator/pwm_out/CMakeLists.txt
+++ /dev/null
@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015-2020 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.
-#
-############################################################################
-
-set(PARAM_PREFIX PWM_MAIN)
-
-if(CONFIG_BOARD_IO)
-	set(PARAM_PREFIX PWM_AUX)
-endif()
-
-px4_add_module(
-	MODULE drivers__pwm_out
-	MAIN pwm_out
-	COMPILE_FLAGS
-		-DPARAM_PREFIX="${PARAM_PREFIX}"
-	SRCS
-		PWMOut.cpp
-		PWMOut.hpp
-	MODULE_CONFIG
-		module.yaml
-	DEPENDS
-		arch_io_pins
-		mixer_module
-	)
diff --git a/apps/peripheral/actuator/pwm_out/Kconfig b/apps/peripheral/actuator/pwm_out/Kconfig
index a263be9a3e..4b2915bb16 100644
--- a/apps/peripheral/actuator/pwm_out/Kconfig
+++ b/apps/peripheral/actuator/pwm_out/Kconfig
@@ -1,5 +1,5 @@
-menuconfig DRIVERS_PWM_OUT
-	bool "pwm_out"
+menuconfig DRV_USING_PWM_OUT
+	bool "pwm out"
 	default n
 	---help---
-		Enable support for pwm_out
\ No newline at end of file
+		Enable support for pwm_out
diff --git a/apps/peripheral/actuator/pwm_out/PWMOut.cpp b/apps/peripheral/actuator/pwm_out/PWMOut.cpp
index 6b482a36f7..682bbbff89 100644
--- a/apps/peripheral/actuator/pwm_out/PWMOut.cpp
+++ b/apps/peripheral/actuator/pwm_out/PWMOut.cpp
@@ -1,321 +1,288 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
-#include "PWMOut.hpp"
+#define LOG_TAG "pwm_out"
+#define LOG_LVL LOG_LVL_INFO
 
-#include <px4_platform_common/sem.hpp>
+#include "PWMOut.hpp"
+#include <rtdevice.h>
 
 PWMOut::PWMOut() :
-	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-	_pwm_mask = ((1u << DIRECT_PWM_OUTPUT_CHANNELS) - 1);
-	_mixing_output.setMaxNumOutputs(DIRECT_PWM_OUTPUT_CHANNELS);
+    OutputModuleInterface(MODULE_NAME, nextpilot::wq_configurations::hp_default) {
+    _pwm_mask = ((1u << DIRECT_PWM_OUTPUT_CHANNELS) - 1);
+    _mixing_output.setMaxNumOutputs(DIRECT_PWM_OUTPUT_CHANNELS);
 
-	// Getting initial parameter values
-	update_params();
+    // Getting initial parameter values
+    update_params();
 }
 
-PWMOut::~PWMOut()
-{
-	/* make sure servos are off */
-	up_pwm_servo_deinit(_pwm_mask);
+PWMOut::~PWMOut() {
+    /* make sure servos are off */
+    up_pwm_servo_deinit(_pwm_mask);
 
-	perf_free(_cycle_perf);
-	perf_free(_interval_perf);
+    perf_free(_cycle_perf);
+    perf_free(_interval_perf);
 }
 
-bool PWMOut::update_pwm_out_state(bool on)
-{
-	if (on && !_pwm_initialized && _pwm_mask != 0) {
+bool PWMOut::update_pwm_out_state(bool on) {
+    if (on && !_pwm_initialized && _pwm_mask != 0) {
+        for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+            _timer_rates[timer] = -1;
 
-		for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
-			_timer_rates[timer] = -1;
+            uint32_t channels = io_timer_get_group(timer);
 
-			uint32_t channels = io_timer_get_group(timer);
+            if (channels == 0) {
+                continue;
+            }
 
-			if (channels == 0) {
-				continue;
-			}
+            char param_name[17];
+            snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-			char param_name[17];
-			snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+            int32_t tim_config = 0;
+            param_t handle     = param_find(param_name);
+            param_get(handle, &tim_config);
 
-			int32_t tim_config = 0;
-			param_t handle = param_find(param_name);
-			param_get(handle, &tim_config);
+            if (tim_config > 0) {
+                _timer_rates[timer] = tim_config;
 
-			if (tim_config > 0) {
-				_timer_rates[timer] = tim_config;
+            } else if (tim_config == -1) { // OneShot
+                _timer_rates[timer] = 0;
 
-			} else if (tim_config == -1) { // OneShot
-				_timer_rates[timer] = 0;
+            } else {
+                _pwm_mask &= ~channels; // don't use for pwm
+            }
+        }
 
-			} else {
-				_pwm_mask &= ~channels; // don't use for pwm
-			}
-		}
+        int ret = up_pwm_servo_init(_pwm_mask);
 
-		int ret = up_pwm_servo_init(_pwm_mask);
+        if (ret < 0) {
+            PX4_ERR("up_pwm_servo_init failed (%i)", ret);
+            return false;
+        }
 
-		if (ret < 0) {
-			PX4_ERR("up_pwm_servo_init failed (%i)", ret);
-			return false;
-		}
+        _pwm_mask = ret;
 
-		_pwm_mask = ret;
+        // set the timer rates
+        for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+            uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
 
-		// set the timer rates
-		for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
-			uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
+            if (channels == 0) {
+                continue;
+            }
 
-			if (channels == 0) {
-				continue;
-			}
+            ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
 
-			ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]);
+            if (ret != 0) {
+                PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
+                _timer_rates[timer]  = -1;
+                _pwm_mask           &= ~channels;
+            }
+        }
 
-			if (ret != 0) {
-				PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret);
-				_timer_rates[timer] = -1;
-				_pwm_mask &= ~channels;
-			}
-		}
+        _pwm_initialized = true;
 
-		_pwm_initialized = true;
+        // disable unused functions
+        for (unsigned i = 0; i < _num_outputs; ++i) {
+            if (((1 << i) & _pwm_mask) == 0) {
+                _mixing_output.disableFunction(i);
+            }
+        }
+    }
 
-		// disable unused functions
-		for (unsigned i = 0; i < _num_outputs; ++i) {
-			if (((1 << i) & _pwm_mask) == 0) {
-				_mixing_output.disableFunction(i);
-			}
-		}
-	}
-
-	up_pwm_servo_arm(on, _pwm_mask);
-	return true;
+    up_pwm_servo_arm(on, _pwm_mask);
+    return true;
 }
 
 bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated)
-{
-	/* output to the servos */
-	if (_pwm_initialized) {
-		for (size_t i = 0; i < num_outputs; i++) {
-			if (!_mixing_output.isFunctionSet(i)) {
-				// do not run any signal on disabled channels
-				outputs[i] = 0;
-			}
-
-			if (_pwm_mask & (1 << i)) {
-				up_pwm_servo_set(i, outputs[i]);
-			}
-		}
-	}
-
-	/* Trigger all timer's channels in Oneshot mode to fire
+                           unsigned num_outputs, unsigned num_control_groups_updated) {
+    /* output to the servos */
+    if (_pwm_initialized) {
+        for (size_t i = 0; i < num_outputs; i++) {
+            if (!_mixing_output.isFunctionSet(i)) {
+                // do not run any signal on disabled channels
+                outputs[i] = 0;
+            }
+
+            if (_pwm_mask & (1 << i)) {
+                up_pwm_servo_set(i, outputs[i]);
+            }
+        }
+    }
+
+    /* Trigger all timer's channels in Oneshot mode to fire
 	 * the oneshots with updated values.
 	 */
-	if (num_control_groups_updated > 0) {
-		up_pwm_update(_pwm_mask);
-	}
+    if (num_control_groups_updated > 0) {
+        up_pwm_update(_pwm_mask);
+    }
 
-	return true;
+    return true;
 }
 
-void PWMOut::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		_mixing_output.unregister();
-
-		exit_and_cleanup();
-		return;
-	}
+void PWMOut::Run() {
+    if (should_exit()) {
+        ScheduleClear();
+        _mixing_output.unregister();
 
-	perf_begin(_cycle_perf);
-	perf_count(_interval_perf);
+        exit_and_cleanup();
+        return;
+    }
 
-	_mixing_output.update();
+    perf_begin(_cycle_perf);
+    perf_count(_interval_perf);
 
-	/* update PWM status if armed or if disarmed PWM values are set */
-	bool pwm_on = true;
+    _mixing_output.update();
 
-	if (_pwm_on != pwm_on) {
-		if (update_pwm_out_state(pwm_on)) {
-			_pwm_on = pwm_on;
-		}
-	}
+    /* update PWM status if armed or if disarmed PWM values are set */
+    bool pwm_on = true;
 
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
+    if (_pwm_on != pwm_on) {
+        if (update_pwm_out_state(pwm_on)) {
+            _pwm_on = pwm_on;
+        }
+    }
 
-		// update parameters from storage
-		update_params();
-	}
+    // check for parameter updates
+    if (_parameter_update_sub.updated()) {
+        // clear update
+        parameter_update_s pupdate;
+        _parameter_update_sub.copy(&pupdate);
 
-	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
-	_mixing_output.updateSubscriptions(true);
+        // update parameters from storage
+        update_params();
+    }
 
-	perf_end(_cycle_perf);
-	_first_update_cycle = false;
-}
-
-int PWMOut::task_spawn(int argc, char *argv[])
-{
-	PWMOut *instance = new PWMOut();
+    // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+    _mixing_output.updateSubscriptions(true);
 
-	if (!instance) {
-		PX4_ERR("alloc failed");
-		return -1;
-	}
-
-	_object.store(instance);
-	_task_id = task_id_is_work_queue;
-	instance->ScheduleNow();
-	return 0;
+    perf_end(_cycle_perf);
+    _first_update_cycle = false;
 }
 
-void PWMOut::update_params()
-{
-	uint32_t previously_set_functions = 0;
-
-	for (size_t i = 0; i < _num_outputs; i++) {
-		previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
-	}
-
-	updateParams();
-
-	// Automatically set PWM configuration when a channel is first assigned
-	if (!_first_update_cycle) {
-		for (size_t i = 0; i < _num_outputs; i++) {
-			if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
-				int32_t output_function;
-
-				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
-					// Servos need PWM rate 50Hz and disramed value 1500us
-					if (output_function >= (int)OutputFunction::Servo1
-					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-						int32_t val = 1500;
-						PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
-						param_set(_mixing_output.disarmedParamHandle(i), &val);
-
-						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-						for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
-
-							uint32_t channels = io_timer_get_group(timer);
-
-							if ((channels & (1u << i)) == 0) {
-								continue;
-							}
-
-							if ((channels & previously_set_functions) == 0) { // None of the channels was set
-								char param_name[17];
-								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
-
-								int32_t tim_config = 0;
-								param_t handle = param_find(param_name);
-
-								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-									tim_config = 50;
-									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-									param_set(handle, &tim_config);
-								}
-							}
-						}
-					}
-
-					// Motors need a minimum value that idles the motor and have a deadzone at the top of the range
-					if (output_function >= (int)OutputFunction::Motor1
-					    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
-						int32_t val = 1100;
-						PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
-						param_set(_mixing_output.minParamHandle(i), &val);
-						val = 1900;
-						PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
-						param_set(_mixing_output.maxParamHandle(i), &val);
-					}
-				}
-			}
-		}
-	}
+// int PWMOut::task_spawn(int argc, char *argv[]) {
+//     PWMOut *instance = new PWMOut();
+
+//     if (!instance) {
+//         PX4_ERR("alloc failed");
+//         return -1;
+//     }
+
+//     _object.store(instance);
+//     _task_id = task_id_is_work_queue;
+//     instance->ScheduleNow();
+//     return 0;
+// }
+
+void PWMOut::update_params() {
+    uint32_t previously_set_functions = 0;
+
+    for (size_t i = 0; i < _num_outputs; i++) {
+        previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
+    }
+
+    updateParams();
+
+    // Automatically set PWM configuration when a channel is first assigned
+    if (!_first_update_cycle) {
+        for (size_t i = 0; i < _num_outputs; i++) {
+            if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
+                int32_t output_function;
+
+                if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+                    // Servos need PWM rate 50Hz and disramed value 1500us
+                    if (output_function >= (int)OutputFunction::Servo1
+                        && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+                        int32_t val = 1500;
+                        PX4_INFO("Setting channel %i disarmed to %i", (int)val, i);
+                        param_set(_mixing_output.disarmedParamHandle(i), &val);
+
+                        // If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+                        for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+                            uint32_t channels = io_timer_get_group(timer);
+
+                            if ((channels & (1u << i)) == 0) {
+                                continue;
+                            }
+
+                            if ((channels & previously_set_functions) == 0) { // None of the channels was set
+                                char param_name[17];
+                                snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+
+                                int32_t tim_config = 0;
+                                param_t handle     = param_find(param_name);
+
+                                if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+                                    tim_config = 50;
+                                    PX4_INFO("setting timer %i to %i Hz", timer, (int)tim_config);
+                                    param_set(handle, &tim_config);
+                                }
+                            }
+                        }
+                    }
+
+                    // Motors need a minimum value that idles the motor and have a deadzone at the top of the range
+                    if (output_function >= (int)OutputFunction::Motor1
+                        && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+                        int32_t val = 1100;
+                        PX4_INFO("Setting channel %i minimum to %i", (int)val, i);
+                        param_set(_mixing_output.minParamHandle(i), &val);
+                        val = 1900;
+                        PX4_INFO("Setting channel %i maximum to %i", (int)val, i);
+                        param_set(_mixing_output.maxParamHandle(i), &val);
+                    }
+                }
+            }
+        }
+    }
 }
 
-int PWMOut::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
+int PWMOut::custom_command(int argc, char *argv[]) {
+    return print_usage("unknown command");
 }
 
-int PWMOut::print_status()
-{
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_interval_perf);
-	_mixing_output.printStatus();
-
-	if (_pwm_initialized) {
-		for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
-			if (_timer_rates[timer] >= 0) {
-				PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
-				uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
-
-				if (channels > 0) {
-					PX4_INFO_RAW(" channels: ");
-
-					for (uint32_t channel = 0; channel < _num_outputs; ++channel) {
-						if ((1 << channel) & channels) {
-							PX4_INFO_RAW("%" PRIu32 " ", channel);
-						}
-					}
-				}
-
-				PX4_INFO_RAW("\n");
-			}
-		}
-	}
-
-	return 0;
+int PWMOut::print_status() {
+    perf_print_counter(_cycle_perf);
+    perf_print_counter(_interval_perf);
+    _mixing_output.printStatus();
+
+    if (_pwm_initialized) {
+        for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+            if (_timer_rates[timer] >= 0) {
+                LOG_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]);
+                uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer);
+
+                if (channels > 0) {
+                    LOG_RAW(" channels: ");
+
+                    for (uint32_t channel = 0; channel < _num_outputs; ++channel) {
+                        if ((1 << channel) & channels) {
+                            LOG_RAW("%" PRIu32 " ", channel);
+                        }
+                    }
+                }
+
+                LOG_RAW("\n");
+            }
+        }
+    }
+
+    return 0;
 }
 
-int PWMOut::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
+int PWMOut::print_usage(const char *reason) {
+    if (reason) {
+        LOG_RAW("%s\n", reason);
+    }
 
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
+    PRINT_MODULE_DESCRIPTION(
+        R"DESCR_STR(
 ### Description
 This module is responsible for driving the output pins. For boards without a separate IO chip
 (eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the
@@ -323,14 +290,24 @@ px4io driver is used for main ones.
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("pwm_out", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    PRINT_MODULE_USAGE_NAME("pwm_out", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+    return 0;
+}
 
-	return 0;
+static int pwm_out_main(int argc, char *argv[]) {
+    return PWMOut::main(argc, argv);
 }
 
-extern "C" __EXPORT int pwm_out_main(int argc, char *argv[])
-{
-	return PWMOut::main(argc, argv);
+MSH_CMD_EXPORT_ALIAS(pwm_out_main, pwm_out, pwm out);
+
+static int pwm_out_start() {
+    const char *argv[] = {"pwm_out", "start"};
+    int         argc   = sizeof(argv) / sizeof(argv[0]);
+
+    return PWMOut::main(argc, (char **)argv);
 }
+
+INIT_APP_EXPORT(pwm_out_start);
diff --git a/apps/peripheral/actuator/pwm_out/PWMOut.hpp b/apps/peripheral/actuator/pwm_out/PWMOut.hpp
index 55d498a8fc..000872c965 100644
--- a/apps/peripheral/actuator/pwm_out/PWMOut.hpp
+++ b/apps/peripheral/actuator/pwm_out/PWMOut.hpp
@@ -1,98 +1,82 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#ifndef __PWMOUT_H__
+#define __PWMOUT_H__
+
 
 #pragma once
 
 #include <float.h>
 #include <math.h>
 
-#include <board_config.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_pwm_output.h>
-#include <lib/mathlib/mathlib.h>
-#include <lib/mixer_module/mixer_module.hpp>
-#include <lib/perf/perf_counter.h>
-#include <px4_arch/io_timer.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/module.h>
+// #include <board_config.h>
+
+#include <hrtimer.h>
+// #include <drivers/drv_pwm_output.h>
+#include <mathlib/mathlib.h>
+#include <mixer_module/mixer_module.hpp>
+#include <perf/perf_counter.h>
+// #include <px4_arch/io_timer.h>
+// #include <px4_platform_common/px4_config.h>
+#include <getopt/getopt.h>
+// #include <px4_platform_common/log.h>
+// #include <px4_platform_common/module.h>
+#include <module/module_params.h>
+#include <module/module_command.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 
 using namespace time_literals;
 
-class PWMOut final : public ModuleBase<PWMOut>, public OutputModuleInterface
-{
+class PWMOut final : public ModuleCommand<PWMOut>, public OutputModuleInterface {
 public:
-	PWMOut();
-	~PWMOut() override;
+    PWMOut();
+    ~PWMOut() override;
 
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
+    /** @see ModuleBase */
+    // static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
+    /** @see ModuleBase */
+    static int custom_command(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
+    /** @see ModuleBase */
+    static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
+    /** @see ModuleBase::print_status() */
+    int print_status() override;
 
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
-			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+    bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+                       unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
-	void Run() override;
+    void Run() override;
 
-	void update_params();
-	bool update_pwm_out_state(bool on);
+    void update_params();
+    bool update_pwm_out_state(bool on);
 
-	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true};
+    MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true};
 
-	int _timer_rates[MAX_IO_TIMERS] {};
+    int _timer_rates[MAX_IO_TIMERS]{};
 
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+    uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	unsigned	_num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
+    unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 
-	bool		_pwm_on{false};
-	uint32_t	_pwm_mask{0};
-	bool		_pwm_initialized{false};
-	bool		_first_update_cycle{true};
+    bool     _pwm_on{false};
+    uint32_t _pwm_mask{0};
+    bool     _pwm_initialized{false};
+    bool     _first_update_cycle{true};
 
-	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 _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")};
+    perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": interval")};
 };
+
+#endif // __PWMOUT_H__
diff --git a/apps/peripheral/actuator/pwm_out/SConscript b/apps/peripheral/actuator/pwm_out/SConscript
new file mode 100644
index 0000000000..c1e45f82ae
--- /dev/null
+++ b/apps/peripheral/actuator/pwm_out/SConscript
@@ -0,0 +1,13 @@
+import os
+import sys
+import subprocess
+from building import *
+
+cwd = GetCurrentDir()
+
+inc = []
+src = Glob("*.cpp")
+
+objs = DefineGroup("drv/battery_status", src, depend=["DRV_USING_PWM_OUT"])
+
+Return("objs")
diff --git a/apps/peripheral/battery/batt_smbus/batt_smbus.cpp b/apps/peripheral/battery/batt_smbus/batt_smbus.cpp
index 8c5b1c7364..4af394f362 100644
--- a/apps/peripheral/battery/batt_smbus/batt_smbus.cpp
+++ b/apps/peripheral/battery/batt_smbus/batt_smbus.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file batt_smbus.h
@@ -48,464 +25,442 @@
 extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
 
 BATT_SMBUS::BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface) :
-	I2CSPIDriver(config),
-	_interface(interface)
-{
-	int32_t battsource = 1;
-	int32_t batt_device_type = static_cast<int32_t>(SMBUS_DEVICE_TYPE::UNDEFINED);
-
-	param_set(param_find("BAT1_SOURCE"), &battsource);
-	param_get(param_find("BAT1_SMBUS_MODEL"), &batt_device_type);
-
-
-	//TODO: probe the device and autodetect its type
-	if ((SMBUS_DEVICE_TYPE)batt_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
-		_device_type = SMBUS_DEVICE_TYPE::BQ40Z80;
-
-	} else {
-		//default
-		_device_type = SMBUS_DEVICE_TYPE::BQ40Z50;
-	}
-
-	_interface->init();
-	// unseal() here to allow an external config script to write to protected flash.
-	// This is neccessary to avoid bus errors due to using standard i2c mode instead of SMbus mode.
-	// The external config script should then seal() the device.
-	unseal();
-}
-
-BATT_SMBUS::~BATT_SMBUS()
-{
-	orb_unadvertise(_batt_topic);
-	perf_free(_cycle);
-
-	if (_interface != nullptr) {
-		delete _interface;
-	}
-
-	int32_t battsource = 0;
-	param_set(param_find("BAT1_SOURCE"), &battsource);
-}
-
-void BATT_SMBUS::RunImpl()
-{
-	int ret = PX4_OK;
-
-	// Temporary variable for storing SMBUS reads.
-	uint16_t result;
-
-	// Read data from sensor.
-	battery_status_s new_report = {};
-
-	// TODO(hyonlim): this driver should support multiple SMBUS going forward.
-	new_report.id = 1;
-
-	// Set time of reading.
-	new_report.timestamp = hrt_absolute_time();
+    I2CSPIDriver(config),
+    _interface(interface) {
+    int32_t battsource       = 1;
+    int32_t batt_device_type = static_cast<int32_t>(SMBUS_DEVICE_TYPE::UNDEFINED);
 
-	new_report.connected = true;
+    param_set(param_find("BAT1_SOURCE"), &battsource);
+    param_get(param_find("BAT1_SMBUS_MODEL"), &batt_device_type);
 
-	ret |= _interface->read_word(BATT_SMBUS_VOLTAGE, result);
 
-	ret |= get_cell_voltages();
+    //TODO: probe the device and autodetect its type
+    if ((SMBUS_DEVICE_TYPE)batt_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
+        _device_type = SMBUS_DEVICE_TYPE::BQ40Z80;
 
-	for (int i = 0; i < _cell_count; i++) {
-		new_report.voltage_cell_v[i] = _cell_voltages[i];
-	}
+    } else {
+        //default
+        _device_type = SMBUS_DEVICE_TYPE::BQ40Z50;
+    }
 
-	// Convert millivolts to volts.
-	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;
-
-	// Read current.
-	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
-
-	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
-	new_report.current_filtered_a = new_report.current_a;
-
-	// Read average current.
-	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
-
-	float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
+    _interface->init();
+    // unseal() here to allow an external config script to write to protected flash.
+    // This is neccessary to avoid bus errors due to using standard i2c mode instead of SMbus mode.
+    // The external config script should then seal() the device.
+    unseal();
+}
 
-	new_report.current_average_a = average_current;
+BATT_SMBUS::~BATT_SMBUS() {
+    orb_unadvertise(_batt_topic);
+    perf_free(_cycle);
 
-	// If current is high, turn under voltage protection off. This is neccessary to prevent
-	// a battery from cutting off while flying with high current near the end of the packs capacity.
-	set_undervoltage_protection(average_current);
+    if (_interface != nullptr) {
+        delete _interface;
+    }
 
-	// Read run time to empty (minutes).
-	ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
-	new_report.time_remaining_s = result * 60;
+    int32_t battsource = 0;
+    param_set(param_find("BAT1_SOURCE"), &battsource);
+}
 
-	// Read average time to empty (minutes).
-	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
-	new_report.average_time_to_empty = result;
+void BATT_SMBUS::RunImpl() {
+    int ret = PX4_OK;
 
-	// Read remaining capacity.
-	ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
+    // Temporary variable for storing SMBUS reads.
+    uint16_t result;
 
-	// Calculate total discharged amount in mah.
-	new_report.discharged_mah = _batt_startup_capacity - (float)result * _c_mult;
+    // Read data from sensor.
+    battery_status_s new_report = {};
 
-	// Read Relative SOC.
-	ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
+    // TODO(hyonlim): this driver should support multiple SMBUS going forward.
+    new_report.id = 1;
 
-	// Normalize 0.0 to 1.0
-	new_report.remaining = (float)result / 100.0f;
+    // Set time of reading.
+    new_report.timestamp = hrt_absolute_time();
 
-	// Read Max Error
-	ret |= _interface->read_word(BATT_SMBUS_MAX_ERROR, result);
-	new_report.max_error = result;
+    new_report.connected = true;
 
-	// Read battery temperature and covert to Celsius.
-	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
-	new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+    ret |= _interface->read_word(BATT_SMBUS_VOLTAGE, result);
 
-	// Only publish if no errors.
-	if (ret == PX4_OK) {
-		new_report.capacity = _batt_capacity;
-		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
-		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
-		new_report.cell_count = _cell_count;
-		new_report.state_of_health = _state_of_health;
+    ret |= get_cell_voltages();
 
-		// Check if max lifetime voltage delta is greater than allowed.
-		if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+    for (int i = 0; i < _cell_count; i++) {
+        new_report.voltage_cell_v[i] = _cell_voltages[i];
+    }
 
-		} else if (new_report.remaining > _low_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
+    // Convert millivolts to volts.
+    new_report.voltage_v          = ((float)result) / 1000.0f;
+    new_report.voltage_filtered_v = new_report.voltage_v;
 
-		} else if (new_report.remaining > _crit_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
+    // Read current.
+    ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
-		} else if (new_report.remaining > _emergency_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+    new_report.current_a          = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
+    new_report.current_filtered_a = new_report.current_a;
 
-		} else {
-			new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
-		}
+    // Read average current.
+    ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
 
-		new_report.interface_error = perf_event_count(_interface->_interface_errors);
+    float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
 
-		int instance = 0;
-		orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);
+    new_report.current_average_a = average_current;
 
-		_last_report = new_report;
-	}
-}
+    // If current is high, turn under voltage protection off. This is neccessary to prevent
+    // a battery from cutting off while flying with high current near the end of the packs capacity.
+    set_undervoltage_protection(average_current);
 
-void BATT_SMBUS::suspend()
-{
-	ScheduleClear();
-}
+    // Read run time to empty (minutes).
+    ret                         |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result);
+    new_report.time_remaining_s  = result * 60;
 
-void BATT_SMBUS::resume()
-{
-	ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
-}
+    // Read average time to empty (minutes).
+    ret                              |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result);
+    new_report.average_time_to_empty  = result;
 
-int BATT_SMBUS::get_cell_voltages()
-{
-	// Temporary variable for storing SMBUS reads.
-	uint16_t result = 0;
-	int ret = PX4_OK;
+    // Read remaining capacity.
+    ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, result);
 
-	if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) {
-		ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE, result);
-		// Convert millivolts to volts.
-		_cell_voltages[0] = ((float)result) / 1000.0f;
+    // Calculate total discharged amount in mah.
+    new_report.discharged_mah = _batt_startup_capacity - (float)result * _c_mult;
 
-		ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE, result);
-		// Convert millivolts to volts.
-		_cell_voltages[1] = ((float)result) / 1000.0f;
+    // Read Relative SOC.
+    ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
 
-		ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE, result);
-		// Convert millivolts to volts.
-		_cell_voltages[2] = ((float)result) / 1000.0f;
+    // Normalize 0.0 to 1.0
+    new_report.remaining = (float)result / 100.0f;
 
-		ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE, result);
-		// Convert millivolts to volts.
-		_cell_voltages[3] = ((float)result) / 1000.0f;
-		_cell_voltages[4] = 0;
-		_cell_voltages[5] = 0;
-		_cell_voltages[6] = 0;
+    // Read Max Error
+    ret                  |= _interface->read_word(BATT_SMBUS_MAX_ERROR, result);
+    new_report.max_error  = result;
 
-	} else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
-		uint8_t DAstatus1[32 + 2] = {}; // 32 bytes of data and 2 bytes of address
+    // Read battery temperature and covert to Celsius.
+    ret                    |= _interface->read_word(BATT_SMBUS_TEMP, result);
+    new_report.temperature  = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
 
-		//TODO: need to consider if set voltages to 0? -1?
-		if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS1, DAstatus1, sizeof(DAstatus1))) {
-			return PX4_ERROR;
-		}
+    // Only publish if no errors.
+    if (ret == PX4_OK) {
+        new_report.capacity               = _batt_capacity;
+        new_report.cycle_count            = _cycle_count;
+        new_report.serial_number          = _serial_number;
+        new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
+        new_report.cell_count             = _cell_count;
+        new_report.state_of_health        = _state_of_health;
 
-		// Convert millivolts to volts.
-		_cell_voltages[0] = ((float)((DAstatus1[1] << 8) | DAstatus1[0]) / 1000.0f);
-		_cell_voltages[1] = ((float)((DAstatus1[3] << 8) | DAstatus1[2]) / 1000.0f);
-		_cell_voltages[2] = ((float)((DAstatus1[5] << 8) | DAstatus1[4]) / 1000.0f);
-		_cell_voltages[3] = ((float)((DAstatus1[7] << 8) | DAstatus1[6]) / 1000.0f);
+        // Check if max lifetime voltage delta is greater than allowed.
+        if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
+            new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
-		_pack_power = ((float)((DAstatus1[29] << 8) | DAstatus1[28]) / 100.0f); //TODO: decide if both needed
-		_pack_average_power = ((float)((DAstatus1[31] << 8) | DAstatus1[30]) / 100.0f);
+        } else if (new_report.remaining > _low_thr) {
+            new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
 
-		uint8_t DAstatus3[18 + 2] = {}; // 18 bytes of data and 2 bytes of address
+        } else if (new_report.remaining > _crit_thr) {
+            new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
 
-		//TODO: need to consider if set voltages to 0? -1?
-		if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS3, DAstatus3, sizeof(DAstatus3))) {
-			return PX4_ERROR;
-		}
+        } else if (new_report.remaining > _emergency_thr) {
+            new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
-		_cell_voltages[4] = ((float)((DAstatus3[1] << 8) | DAstatus3[0]) / 1000.0f);
-		_cell_voltages[5] = ((float)((DAstatus3[7] << 8) | DAstatus3[6]) / 1000.0f);
-		_cell_voltages[6] = ((float)((DAstatus3[13] << 8) | DAstatus3[12]) / 1000.0f);
+        } else {
+            new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
+        }
 
-	}
+        new_report.interface_error = perf_event_count(_interface->_interface_errors);
 
-	//Calculate max cell delta
-	_min_cell_voltage = _cell_voltages[0];
-	float max_cell_voltage = _cell_voltages[0];
+        int instance = 0;
+        orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);
 
-	for (uint8_t i = 1; (i < _cell_count && i < (sizeof(_cell_voltages) / sizeof(_cell_voltages[0]))); i++) {
-		_min_cell_voltage = math::min(_min_cell_voltage, _cell_voltages[i]);
-		max_cell_voltage = math::max(max_cell_voltage, _cell_voltages[i]);
-	}
+        _last_report = new_report;
+    }
+}
 
-	// Calculate the max difference between the min and max cells with complementary filter.
-	_max_cell_voltage_delta = (0.5f * (max_cell_voltage - _min_cell_voltage)) +
-				  (0.5f * _last_report.max_cell_voltage_delta);
+void BATT_SMBUS::suspend() {
+    ScheduleClear();
+}
 
-	return ret;
+void BATT_SMBUS::resume() {
+    ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
 }
 
-void BATT_SMBUS::set_undervoltage_protection(float average_current)
-{
-	// Disable undervoltage protection if armed. Enable if disarmed and cell voltage is above limit.
-	if (average_current > BATT_CURRENT_UNDERVOLTAGE_THRESHOLD) {
-		if (_cell_undervoltage_protection_status != 0) {
-			// Disable undervoltage protection
-			uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED;
-			uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS;
-
-			if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) {
-				_cell_undervoltage_protection_status = 0;
-				PX4_WARN("Disabled CUV");
-
-			} else {
-				PX4_WARN("Failed to disable CUV");
-			}
-		}
-
-	} else {
-		if (_cell_undervoltage_protection_status == 0) {
-			if (_min_cell_voltage > BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD) {
-				// Enable undervoltage protection
-				uint8_t protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT;
-				uint16_t address = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS;
-
-				if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) {
-					_cell_undervoltage_protection_status = 1;
-					PX4_WARN("Enabled CUV");
-
-				} else {
-					PX4_WARN("Failed to enable CUV");
-				}
-			}
-		}
-	}
+int BATT_SMBUS::get_cell_voltages() {
+    // Temporary variable for storing SMBUS reads.
+    uint16_t result = 0;
+    int      ret    = PX4_OK;
+
+    if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) {
+        ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE, result);
+        // Convert millivolts to volts.
+        _cell_voltages[0] = ((float)result) / 1000.0f;
+
+        ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE, result);
+        // Convert millivolts to volts.
+        _cell_voltages[1] = ((float)result) / 1000.0f;
+
+        ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE, result);
+        // Convert millivolts to volts.
+        _cell_voltages[2] = ((float)result) / 1000.0f;
+
+        ret |= _interface->read_word(BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE, result);
+        // Convert millivolts to volts.
+        _cell_voltages[3] = ((float)result) / 1000.0f;
+        _cell_voltages[4] = 0;
+        _cell_voltages[5] = 0;
+        _cell_voltages[6] = 0;
+
+    } else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
+        uint8_t DAstatus1[32 + 2] = {}; // 32 bytes of data and 2 bytes of address
+
+        //TODO: need to consider if set voltages to 0? -1?
+        if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS1, DAstatus1, sizeof(DAstatus1))) {
+            return PX4_ERROR;
+        }
+
+        // Convert millivolts to volts.
+        _cell_voltages[0] = ((float)((DAstatus1[1] << 8) | DAstatus1[0]) / 1000.0f);
+        _cell_voltages[1] = ((float)((DAstatus1[3] << 8) | DAstatus1[2]) / 1000.0f);
+        _cell_voltages[2] = ((float)((DAstatus1[5] << 8) | DAstatus1[4]) / 1000.0f);
+        _cell_voltages[3] = ((float)((DAstatus1[7] << 8) | DAstatus1[6]) / 1000.0f);
+
+        _pack_power         = ((float)((DAstatus1[29] << 8) | DAstatus1[28]) / 100.0f); //TODO: decide if both needed
+        _pack_average_power = ((float)((DAstatus1[31] << 8) | DAstatus1[30]) / 100.0f);
+
+        uint8_t DAstatus3[18 + 2] = {}; // 18 bytes of data and 2 bytes of address
+
+        //TODO: need to consider if set voltages to 0? -1?
+        if (PX4_OK != manufacturer_read(BATT_SMBUS_DASTATUS3, DAstatus3, sizeof(DAstatus3))) {
+            return PX4_ERROR;
+        }
+
+        _cell_voltages[4] = ((float)((DAstatus3[1] << 8) | DAstatus3[0]) / 1000.0f);
+        _cell_voltages[5] = ((float)((DAstatus3[7] << 8) | DAstatus3[6]) / 1000.0f);
+        _cell_voltages[6] = ((float)((DAstatus3[13] << 8) | DAstatus3[12]) / 1000.0f);
+    }
+
+    //Calculate max cell delta
+    _min_cell_voltage      = _cell_voltages[0];
+    float max_cell_voltage = _cell_voltages[0];
+
+    for (uint8_t i = 1; (i < _cell_count && i < (sizeof(_cell_voltages) / sizeof(_cell_voltages[0]))); i++) {
+        _min_cell_voltage = math::min(_min_cell_voltage, _cell_voltages[i]);
+        max_cell_voltage  = math::max(max_cell_voltage, _cell_voltages[i]);
+    }
+
+    // Calculate the max difference between the min and max cells with complementary filter.
+    _max_cell_voltage_delta = (0.5f * (max_cell_voltage - _min_cell_voltage)) + (0.5f * _last_report.max_cell_voltage_delta);
+
+    return ret;
+}
 
+void BATT_SMBUS::set_undervoltage_protection(float average_current) {
+    // Disable undervoltage protection if armed. Enable if disarmed and cell voltage is above limit.
+    if (average_current > BATT_CURRENT_UNDERVOLTAGE_THRESHOLD) {
+        if (_cell_undervoltage_protection_status != 0) {
+            // Disable undervoltage protection
+            uint8_t  protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED;
+            uint16_t address           = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS;
+
+            if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) {
+                _cell_undervoltage_protection_status = 0;
+                PX4_WARN("Disabled CUV");
+
+            } else {
+                PX4_WARN("Failed to disable CUV");
+            }
+        }
+
+    } else {
+        if (_cell_undervoltage_protection_status == 0) {
+            if (_min_cell_voltage > BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD) {
+                // Enable undervoltage protection
+                uint8_t  protections_a_tmp = BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT;
+                uint16_t address           = BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS;
+
+                if (dataflash_write(address, &protections_a_tmp, 1) == PX4_OK) {
+                    _cell_undervoltage_protection_status = 1;
+                    PX4_WARN("Enabled CUV");
+
+                } else {
+                    PX4_WARN("Failed to enable CUV");
+                }
+            }
+        }
+    }
 }
 
 //@NOTE: Currently unused, could be helpful for debugging a parameter set though.
-int BATT_SMBUS::dataflash_read(const uint16_t address, void *data, const unsigned length)
-{
-	uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
+int BATT_SMBUS::dataflash_read(const uint16_t address, void *data, const unsigned length) {
+    uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
 
-	int ret = _interface->block_write(code, &address, 2, true);
+    int ret = _interface->block_write(code, &address, 2, true);
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	ret = _interface->block_read(code, data, length, true);
+    ret = _interface->block_read(code, data, length, true);
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::dataflash_write(const uint16_t address, void *data, const unsigned length)
-{
-	uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
+int BATT_SMBUS::dataflash_write(const uint16_t address, void *data, const unsigned length) {
+    uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
 
-	uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {};
+    uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {};
 
-	tx_buf[0] = address & 0xff;
-	tx_buf[1] = (address >> 8) & 0xff;
+    tx_buf[0] = address & 0xff;
+    tx_buf[1] = (address >> 8) & 0xff;
 
-	if (length > MAC_DATA_BUFFER_SIZE) {
-		return PX4_ERROR;
-	}
+    if (length > MAC_DATA_BUFFER_SIZE) {
+        return PX4_ERROR;
+    }
 
-	memcpy(&tx_buf[2], data, length);
+    memcpy(&tx_buf[2], data, length);
 
-	// code (1), byte_count (1), addr(2), data(32) + pec
-	int ret = _interface->block_write(code, tx_buf, length + 2, false);
+    // code (1), byte_count (1), addr(2), data(32) + pec
+    int ret = _interface->block_write(code, tx_buf, length + 2, false);
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::get_startup_info()
-{
-	int ret = PX4_OK;
+int BATT_SMBUS::get_startup_info() {
+    int ret = PX4_OK;
 
-	// Read battery threshold params on startup.
-	// TODO: support instances
-	param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
-	param_get(param_find("BAT_LOW_THR"), &_low_thr);
-	param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
-	param_get(param_find("BAT1_C_MULT"), &_c_mult);
+    // Read battery threshold params on startup.
+    // TODO: support instances
+    param_get(param_find("BAT_CRIT_THR"), &_crit_thr);
+    param_get(param_find("BAT_LOW_THR"), &_low_thr);
+    param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr);
+    param_get(param_find("BAT1_C_MULT"), &_c_mult);
 
-	int32_t cell_count_param = 0;
-	param_get(param_find("BAT1_N_CELLS"), &cell_count_param);
-	_cell_count = math::min((uint8_t)cell_count_param, MAX_NUM_OF_CELLS);
+    int32_t cell_count_param = 0;
+    param_get(param_find("BAT1_N_CELLS"), &cell_count_param);
+    _cell_count = math::min((uint8_t)cell_count_param, MAX_NUM_OF_CELLS);
 
-	ret |= _interface->block_read(BATT_SMBUS_MANUFACTURER_NAME, _manufacturer_name, BATT_SMBUS_MANUFACTURER_NAME_SIZE,
-				      true);
-	_manufacturer_name[sizeof(_manufacturer_name) - 1] = '\0';
+    ret                                                |= _interface->block_read(BATT_SMBUS_MANUFACTURER_NAME, _manufacturer_name, BATT_SMBUS_MANUFACTURER_NAME_SIZE,
+                                                                                 true);
+    _manufacturer_name[sizeof(_manufacturer_name) - 1]  = '\0';
 
-	uint16_t serial_num;
-	ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num);
+    uint16_t serial_num;
+    ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, serial_num);
 
-	uint16_t remaining_cap;
-	ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap);
+    uint16_t remaining_cap;
+    ret |= _interface->read_word(BATT_SMBUS_REMAINING_CAPACITY, remaining_cap);
 
-	uint16_t cycle_count;
-	ret |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count);
+    uint16_t cycle_count;
+    ret |= _interface->read_word(BATT_SMBUS_CYCLE_COUNT, cycle_count);
 
-	uint16_t full_cap;
-	ret |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap);
+    uint16_t full_cap;
+    ret |= _interface->read_word(BATT_SMBUS_FULL_CHARGE_CAPACITY, full_cap);
 
-	uint16_t manufacture_date;
-	ret |= _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, manufacture_date);
+    uint16_t manufacture_date;
+    ret |= _interface->read_word(BATT_SMBUS_MANUFACTURE_DATE, manufacture_date);
 
-	uint16_t state_of_health;
-	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);
+    uint16_t state_of_health;
+    ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);
 
-	if (!ret) {
-		_serial_number = serial_num;
-		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
-		_cycle_count = cycle_count;
-		_batt_capacity = (uint16_t)((float)full_cap * _c_mult);
-		_manufacture_date = manufacture_date;
-		_state_of_health = state_of_health;
-	}
+    if (!ret) {
+        _serial_number         = serial_num;
+        _batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
+        _cycle_count           = cycle_count;
+        _batt_capacity         = (uint16_t)((float)full_cap * _c_mult);
+        _manufacture_date      = manufacture_date;
+        _state_of_health       = state_of_health;
+    }
 
-	if (lifetime_data_flush() == PX4_OK) {
-		// Flush needs time to complete, otherwise device is busy. 100ms not enough, 200ms works.
-		px4_usleep(200_ms);
+    if (lifetime_data_flush() == PX4_OK) {
+        // Flush needs time to complete, otherwise device is busy. 100ms not enough, 200ms works.
+        px4_usleep(200_ms);
 
-		if (lifetime_read_block_one() == PX4_OK) {
-			if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
-				PX4_WARN("Battery Damaged Will Not Fly. Lifetime max voltage difference: %4.2f",
-					 (double)_lifetime_max_delta_cell_voltage);
-			}
-		}
+        if (lifetime_read_block_one() == PX4_OK) {
+            if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
+                PX4_WARN("Battery Damaged Will Not Fly. Lifetime max voltage difference: %4.2f",
+                         (double)_lifetime_max_delta_cell_voltage);
+            }
+        }
 
-	} else {
-		PX4_WARN("Failed to flush lifetime data");
-	}
+    } else {
+        PX4_WARN("Failed to flush lifetime data");
+    }
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length)
-{
-	uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
+int BATT_SMBUS::manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length) {
+    uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
 
-	uint8_t address[2] = {};
-	address[0] = ((uint8_t *)&cmd_code)[0];
-	address[1] = ((uint8_t *)&cmd_code)[1];
+    uint8_t address[2] = {};
+    address[0]         = ((uint8_t *)&cmd_code)[0];
+    address[1]         = ((uint8_t *)&cmd_code)[1];
 
-	int ret = _interface->block_write(code, address, 2, false);
+    int ret = _interface->block_write(code, address, 2, false);
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	ret = _interface->block_read(code, data, length, true);
-	memmove(data, &((uint8_t *)data)[2], length - 2); // remove the address bytes
+    ret = _interface->block_read(code, data, length, true);
+    memmove(data, &((uint8_t *)data)[2], length - 2); // remove the address bytes
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length)
-{
-	uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
+int BATT_SMBUS::manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length) {
+    uint8_t code = BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS;
 
-	uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {};
-	tx_buf[0] = cmd_code & 0xff;
-	tx_buf[1] = (cmd_code >> 8) & 0xff;
+    uint8_t tx_buf[MAC_DATA_BUFFER_SIZE + 2] = {};
+    tx_buf[0]                                = cmd_code & 0xff;
+    tx_buf[1]                                = (cmd_code >> 8) & 0xff;
 
-	if (data != nullptr && length <= MAC_DATA_BUFFER_SIZE) {
-		memcpy(&tx_buf[2], data, length);
-	}
+    if (data != nullptr && length <= MAC_DATA_BUFFER_SIZE) {
+        memcpy(&tx_buf[2], data, length);
+    }
 
-	int ret = _interface->block_write(code, tx_buf, length + 2, false);
+    int ret = _interface->block_write(code, tx_buf, length + 2, false);
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::unseal()
-{
-	// See bq40z50 technical reference.
-	uint16_t keys[2] = {0x0414, 0x3672};
+int BATT_SMBUS::unseal() {
+    // See bq40z50 technical reference.
+    uint16_t keys[2] = {0x0414, 0x3672};
 
-	int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[0]);
+    int ret = _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[0]);
 
-	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[1]);
+    ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, keys[1]);
 
-	return ret;
+    return ret;
 }
 
-int BATT_SMBUS::seal()
-{
-	// See bq40z50 technical reference.
-	return manufacturer_write(BATT_SMBUS_SEAL, nullptr, 0);
+int BATT_SMBUS::seal() {
+    // See bq40z50 technical reference.
+    return manufacturer_write(BATT_SMBUS_SEAL, nullptr, 0);
 }
 
-int BATT_SMBUS::lifetime_data_flush()
-{
-	return manufacturer_write(BATT_SMBUS_LIFETIME_FLUSH, nullptr, 0);
+int BATT_SMBUS::lifetime_data_flush() {
+    return manufacturer_write(BATT_SMBUS_LIFETIME_FLUSH, nullptr, 0);
 }
 
-int BATT_SMBUS::lifetime_read_block_one()
-{
-	uint8_t lifetime_block_one[32 + 2] = {}; // 32 bytes of data and 2 bytes of address
-
-	if (PX4_OK != manufacturer_read(BATT_SMBUS_LIFETIME_BLOCK_ONE, lifetime_block_one, sizeof(lifetime_block_one))) {
-		PX4_INFO("Failed to read lifetime block 1.");
-		return PX4_ERROR;
-	}
-
-	//Get max cell voltage delta and convert from mV to V.
-	if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) {
+int BATT_SMBUS::lifetime_read_block_one() {
+    uint8_t lifetime_block_one[32 + 2] = {}; // 32 bytes of data and 2 bytes of address
 
-		_lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[17] << 8 | lifetime_block_one[16]) / 1000.0f;
+    if (PX4_OK != manufacturer_read(BATT_SMBUS_LIFETIME_BLOCK_ONE, lifetime_block_one, sizeof(lifetime_block_one))) {
+        PX4_INFO("Failed to read lifetime block 1.");
+        return PX4_ERROR;
+    }
 
-	} else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
+    //Get max cell voltage delta and convert from mV to V.
+    if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z50) {
+        _lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[17] << 8 | lifetime_block_one[16]) / 1000.0f;
 
-		_lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[29] << 8 | lifetime_block_one[28]) / 1000.0f;
-	}
+    } else if (_device_type == SMBUS_DEVICE_TYPE::BQ40Z80) {
+        _lifetime_max_delta_cell_voltage = (float)(lifetime_block_one[29] << 8 | lifetime_block_one[28]) / 1000.0f;
+    }
 
-	PX4_INFO("Max Cell Delta: %4.2f", (double)_lifetime_max_delta_cell_voltage);
+    PX4_INFO("Max Cell Delta: %4.2f", (double)_lifetime_max_delta_cell_voltage);
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-void BATT_SMBUS::print_usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
+void BATT_SMBUS::print_usage() {
+    PRINT_MODULE_DESCRIPTION(
+        R"DESCR_STR(
 ### Description
 Smart battery driver for the BQ40Z50 fuel gauge IC.
 
@@ -515,160 +470,155 @@ To write to flash to set parameters. address, number_of_bytes, byte0, ... , byte
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("batt_smbus", "driver");
+    PRINT_MODULE_USAGE_NAME("batt_smbus", "driver");
 
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0B);
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0B);
 
-	PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info.");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands.");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("seal", "Seals the devices flash memory to disable write_flash commands.");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the driver from rescheduling the cycle.");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from suspension.");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("man_info", "Prints manufacturer info.");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("unseal", "Unseals the devices flash memory to enable write_flash commands.");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("seal", "Seals the devices flash memory to disable write_flash commands.");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("suspend", "Suspends the driver from rescheduling the cycle.");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("resume", "Resumes the driver from suspension.");
 
-	PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command.");
-	PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true);
-	PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to send.", true);
-	PRINT_MODULE_USAGE_ARG("data[0]...data[n]", "One byte of data at a time separated by spaces.", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    PRINT_MODULE_USAGE_COMMAND_DESCR("write_flash", "Writes to flash. The device must first be unsealed with the unseal command.");
+    PRINT_MODULE_USAGE_ARG("address", "The address to start writing.", true);
+    PRINT_MODULE_USAGE_ARG("number of bytes", "Number of bytes to send.", true);
+    PRINT_MODULE_USAGE_ARG("data[0]...data[n]", "One byte of data at a time separated by spaces.", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-I2CSPIDriverBase *BATT_SMBUS::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	SMBus *interface = new SMBus(DRV_BAT_DEVTYPE_SMBUS, config.bus, config.i2c_address);
-	if (interface == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-	BATT_SMBUS *instance = new BATT_SMBUS(config, interface);
+I2CSPIDriverBase *BATT_SMBUS::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    SMBus *interface = new SMBus(DRV_BAT_DEVTYPE_SMBUS, config.bus, config.i2c_address);
+    if (interface == nullptr) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
+    BATT_SMBUS *instance = new BATT_SMBUS(config, interface);
 
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
+    if (instance == nullptr) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
 
-	int ret = instance->get_startup_info();
+    int ret = instance->get_startup_info();
 
-	if (ret != PX4_OK) {
-		delete instance;
-		return nullptr;
-	}
+    if (ret != PX4_OK) {
+        delete instance;
+        return nullptr;
+    }
 
-	instance->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
+    instance->ScheduleOnInterval(BATT_SMBUS_MEASUREMENT_INTERVAL_US);
 
-	return instance;
+    return instance;
 }
 
-void
-BATT_SMBUS::custom_method(const BusCLIArguments &cli)
-{
-	switch(cli.custom1) {
-		case 1: {
-			PX4_INFO("The manufacturer name: %s", _manufacturer_name);
-			PX4_INFO("The manufacturer date: %" PRId16, _manufacture_date);
-			PX4_INFO("The serial number: %d" PRId16, _serial_number);
-		}
-			break;
-		case 2:
-			unseal();
-			break;
-		case 3:
-			seal();
-			break;
-		case 4:
-			suspend();
-			break;
-		case 5:
-			resume();
-			break;
-		case 6:
-			if (cli.custom_data) {
-				unsigned address = cli.custom2;
-				uint8_t *tx_buf = (uint8_t*)cli.custom_data;
-				unsigned length = tx_buf[0];
-
-				if (PX4_OK != dataflash_write(address, tx_buf+1, length)) {
-					PX4_ERR("Dataflash write failed: %u", address);
-				}
-				px4_usleep(100_ms);
-			}
-			break;
-	}
+void BATT_SMBUS::custom_method(const BusCLIArguments &cli) {
+    switch (cli.custom1) {
+    case 1: {
+        PX4_INFO("The manufacturer name: %s", _manufacturer_name);
+        PX4_INFO("The manufacturer date: %" PRId16, _manufacture_date);
+        PX4_INFO("The serial number: %d" PRId16, _serial_number);
+    } break;
+    case 2:
+        unseal();
+        break;
+    case 3:
+        seal();
+        break;
+    case 4:
+        suspend();
+        break;
+    case 5:
+        resume();
+        break;
+    case 6:
+        if (cli.custom_data) {
+            unsigned address = cli.custom2;
+            uint8_t *tx_buf  = (uint8_t *)cli.custom_data;
+            unsigned length  = tx_buf[0];
+
+            if (PX4_OK != dataflash_write(address, tx_buf + 1, length)) {
+                PX4_ERR("Dataflash write failed: %u", address);
+            }
+            px4_usleep(100_ms);
+        }
+        break;
+    }
 }
 
-extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[])
-{
-	using ThisDriver = BATT_SMBUS;
-	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = 100000;
-	cli.i2c_address = BATT_SMBUS_ADDR;
-
-	const char *verb = cli.parseDefaultArguments(argc, argv);
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BAT_DEVTYPE_SMBUS);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	if (!strcmp(verb, "man_info")) {
-		cli.custom1 = 1;
-		return ThisDriver::module_custom_method(cli, iterator, false);
-	}
-	if (!strcmp(verb, "unseal")) {
-		cli.custom1 = 2;
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-	if (!strcmp(verb, "seal")) {
-		cli.custom1 = 3;
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-	if (!strcmp(verb, "suspend")) {
-		cli.custom1 = 4;
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-	if (!strcmp(verb, "resume")) {
-		cli.custom1 = 5;
-		return ThisDriver::module_custom_method(cli, iterator);
-	}
-	if (!strcmp(verb, "write_flash")) {
-		cli.custom1 = 6;
-		if (argc >= 3) {
-			uint16_t address = atoi(argv[1]);
-			unsigned length = atoi(argv[2]);
-			uint8_t tx_buf[33];
-			cli.custom_data = &tx_buf;
-
-			if (length > 32) {
-				PX4_WARN("Data length out of range: Max 32 bytes");
-				return 1;
-			}
-
-			tx_buf[0] = length;
-			// Data needs to be fed in 1 byte (0x01) at a time.
-			for (unsigned i = 0; i < length; i++) {
-				if ((unsigned)argc <= 3 + i) {
-					tx_buf[i+1] = atoi(argv[3 + i]);
-				}
-			}
-			cli.custom2 = address;
-			return ThisDriver::module_custom_method(cli, iterator);
-		}
-	}
-
-	ThisDriver::print_usage();
-	return -1;
+extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]) {
+    using ThisDriver = BATT_SMBUS;
+    BusCLIArguments cli{true, false};
+    cli.default_i2c_frequency = 100000;
+    cli.i2c_address           = BATT_SMBUS_ADDR;
+
+    const char *verb = cli.parseDefaultArguments(argc, argv);
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
+
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BAT_DEVTYPE_SMBUS);
+
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
+
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
+
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
+
+    if (!strcmp(verb, "man_info")) {
+        cli.custom1 = 1;
+        return ThisDriver::module_custom_method(cli, iterator, false);
+    }
+    if (!strcmp(verb, "unseal")) {
+        cli.custom1 = 2;
+        return ThisDriver::module_custom_method(cli, iterator);
+    }
+    if (!strcmp(verb, "seal")) {
+        cli.custom1 = 3;
+        return ThisDriver::module_custom_method(cli, iterator);
+    }
+    if (!strcmp(verb, "suspend")) {
+        cli.custom1 = 4;
+        return ThisDriver::module_custom_method(cli, iterator);
+    }
+    if (!strcmp(verb, "resume")) {
+        cli.custom1 = 5;
+        return ThisDriver::module_custom_method(cli, iterator);
+    }
+    if (!strcmp(verb, "write_flash")) {
+        cli.custom1 = 6;
+        if (argc >= 3) {
+            uint16_t address = atoi(argv[1]);
+            unsigned length  = atoi(argv[2]);
+            uint8_t  tx_buf[33];
+            cli.custom_data = &tx_buf;
+
+            if (length > 32) {
+                PX4_WARN("Data length out of range: Max 32 bytes");
+                return 1;
+            }
+
+            tx_buf[0] = length;
+            // Data needs to be fed in 1 byte (0x01) at a time.
+            for (unsigned i = 0; i < length; i++) {
+                if ((unsigned)argc <= 3 + i) {
+                    tx_buf[i + 1] = atoi(argv[3 + i]);
+                }
+            }
+            cli.custom2 = address;
+            return ThisDriver::module_custom_method(cli, iterator);
+        }
+    }
+
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/battery/batt_smbus/batt_smbus.h b/apps/peripheral/battery/batt_smbus/batt_smbus.h
index 1915865c80..d8a8347110 100644
--- a/apps/peripheral/battery/batt_smbus/batt_smbus.h
+++ b/apps/peripheral/battery/batt_smbus/batt_smbus.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file batt_smbus.h
@@ -58,234 +35,232 @@
 
 using namespace time_literals;
 
-#define BATT_SMBUS_MEASUREMENT_INTERVAL_US              100_ms         ///< time in microseconds, measure at 10Hz
-
-#define MAC_DATA_BUFFER_SIZE                            32
-
-#define BATT_CELL_VOLTAGE_THRESHOLD_RTL                 0.5f            ///< Threshold in volts to RTL if cells are imbalanced
-#define BATT_CELL_VOLTAGE_THRESHOLD_FAILED              1.5f            ///< Threshold in volts to Land if cells are imbalanced
-
-#define BATT_CURRENT_UNDERVOLTAGE_THRESHOLD             5.0f            ///< Threshold in amps to disable undervoltage protection
-#define BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD             3.4f            ///< Threshold in volts to re-enable undervoltage protection
-
-#define BATT_SMBUS_ADDR                                 0x0B            ///< Default 7 bit address I2C address. 8 bit = 0x16
-
-#define BATT_SMBUS_TEMP                                 0x08            ///< temperature register
-#define BATT_SMBUS_VOLTAGE                              0x09            ///< voltage register
-#define BATT_SMBUS_CURRENT                              0x0A            ///< current register
-#define BATT_SMBUS_AVERAGE_CURRENT                      0x0B            ///< average current register
-#define BATT_SMBUS_MAX_ERROR                            0x0C            ///< max error
-#define BATT_SMBUS_RELATIVE_SOC                         0x0D            ///< Relative State Of Charge
-#define BATT_SMBUS_ABSOLUTE_SOC                         0x0E            ///< Absolute State of charge
-#define BATT_SMBUS_REMAINING_CAPACITY                   0x0F            ///< predicted remaining battery capacity as a percentage
-#define BATT_SMBUS_FULL_CHARGE_CAPACITY                 0x10            ///< capacity when fully charged
-#define BATT_SMBUS_RUN_TIME_TO_EMPTY                    0x11            ///< predicted remaining battery capacity based on the present rate of discharge in min
-#define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY                0x12            ///< predicted remaining battery capacity based on the present rate of discharge in min
-#define BATT_SMBUS_CYCLE_COUNT                          0x17            ///< number of cycles the battery has experienced
-#define BATT_SMBUS_DESIGN_CAPACITY                      0x18            ///< design capacity register
-#define BATT_SMBUS_DESIGN_VOLTAGE                       0x19            ///< design voltage register
-#define BATT_SMBUS_MANUFACTURER_NAME                    0x20            ///< manufacturer name
-#define BATT_SMBUS_MANUFACTURER_NAME_SIZE               21              ///< manufacturer name data size
-#define BATT_SMBUS_MANUFACTURE_DATE                     0x1B            ///< manufacture date register
-#define BATT_SMBUS_SERIAL_NUMBER                        0x1C            ///< serial number register
-
-#define BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE               0x3C
-#define BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE               0x3D
-#define BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE               0x3E
-#define BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE               0x3F
-
-#define BATT_SMBUS_BQ40Z80_CELL_7_VOLTAGE               0x3C
-#define BATT_SMBUS_BQ40Z80_CELL_6_VOLTAGE               0x3D
-#define BATT_SMBUS_BQ40Z80_CELL_5_VOLTAGE               0x3E
-#define BATT_SMBUS_BQ40Z80_CELL_4_VOLTAGE               0x3F
-
-#define BATT_SMBUS_STATE_OF_HEALTH                      0x4F            ///< State of Health. The SOH information of the battery in percentage of Design Capacity
-
-#define BATT_SMBUS_MANUFACTURER_ACCESS                  0x00
-#define BATT_SMBUS_MANUFACTURER_DATA                    0x23
-#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS            0x44
-
-#define BATT_SMBUS_SECURITY_KEYS                        0x0035
-
-#define BATT_SMBUS_LIFETIME_FLUSH                       0x002E
-#define BATT_SMBUS_LIFETIME_BLOCK_ONE                   0x0060
-#define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS        0x4938
-#define BATT_SMBUS_SEAL                                 0x0030
-#define BATT_SMBUS_DASTATUS1                            0x0071
-#define BATT_SMBUS_DASTATUS2                            0x0072
-#define BATT_SMBUS_DASTATUS3                            0x007B
-
-#define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT        0xcf
-#define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED   0xce
+#define BATT_SMBUS_MEASUREMENT_INTERVAL_US            100_ms ///< time in microseconds, measure at 10Hz
+
+#define MAC_DATA_BUFFER_SIZE                          32
+
+#define BATT_CELL_VOLTAGE_THRESHOLD_RTL               0.5f ///< Threshold in volts to RTL if cells are imbalanced
+#define BATT_CELL_VOLTAGE_THRESHOLD_FAILED            1.5f ///< Threshold in volts to Land if cells are imbalanced
+
+#define BATT_CURRENT_UNDERVOLTAGE_THRESHOLD           5.0f ///< Threshold in amps to disable undervoltage protection
+#define BATT_VOLTAGE_UNDERVOLTAGE_THRESHOLD           3.4f ///< Threshold in volts to re-enable undervoltage protection
+
+#define BATT_SMBUS_ADDR                               0x0B ///< Default 7 bit address I2C address. 8 bit = 0x16
+
+#define BATT_SMBUS_TEMP                               0x08 ///< temperature register
+#define BATT_SMBUS_VOLTAGE                            0x09 ///< voltage register
+#define BATT_SMBUS_CURRENT                            0x0A ///< current register
+#define BATT_SMBUS_AVERAGE_CURRENT                    0x0B ///< average current register
+#define BATT_SMBUS_MAX_ERROR                          0x0C ///< max error
+#define BATT_SMBUS_RELATIVE_SOC                       0x0D ///< Relative State Of Charge
+#define BATT_SMBUS_ABSOLUTE_SOC                       0x0E ///< Absolute State of charge
+#define BATT_SMBUS_REMAINING_CAPACITY                 0x0F ///< predicted remaining battery capacity as a percentage
+#define BATT_SMBUS_FULL_CHARGE_CAPACITY               0x10 ///< capacity when fully charged
+#define BATT_SMBUS_RUN_TIME_TO_EMPTY                  0x11 ///< predicted remaining battery capacity based on the present rate of discharge in min
+#define BATT_SMBUS_AVERAGE_TIME_TO_EMPTY              0x12 ///< predicted remaining battery capacity based on the present rate of discharge in min
+#define BATT_SMBUS_CYCLE_COUNT                        0x17 ///< number of cycles the battery has experienced
+#define BATT_SMBUS_DESIGN_CAPACITY                    0x18 ///< design capacity register
+#define BATT_SMBUS_DESIGN_VOLTAGE                     0x19 ///< design voltage register
+#define BATT_SMBUS_MANUFACTURER_NAME                  0x20 ///< manufacturer name
+#define BATT_SMBUS_MANUFACTURER_NAME_SIZE             21   ///< manufacturer name data size
+#define BATT_SMBUS_MANUFACTURE_DATE                   0x1B ///< manufacture date register
+#define BATT_SMBUS_SERIAL_NUMBER                      0x1C ///< serial number register
+
+#define BATT_SMBUS_BQ40Z50_CELL_4_VOLTAGE             0x3C
+#define BATT_SMBUS_BQ40Z50_CELL_3_VOLTAGE             0x3D
+#define BATT_SMBUS_BQ40Z50_CELL_2_VOLTAGE             0x3E
+#define BATT_SMBUS_BQ40Z50_CELL_1_VOLTAGE             0x3F
+
+#define BATT_SMBUS_BQ40Z80_CELL_7_VOLTAGE             0x3C
+#define BATT_SMBUS_BQ40Z80_CELL_6_VOLTAGE             0x3D
+#define BATT_SMBUS_BQ40Z80_CELL_5_VOLTAGE             0x3E
+#define BATT_SMBUS_BQ40Z80_CELL_4_VOLTAGE             0x3F
+
+#define BATT_SMBUS_STATE_OF_HEALTH                    0x4F ///< State of Health. The SOH information of the battery in percentage of Design Capacity
+
+#define BATT_SMBUS_MANUFACTURER_ACCESS                0x00
+#define BATT_SMBUS_MANUFACTURER_DATA                  0x23
+#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS          0x44
+
+#define BATT_SMBUS_SECURITY_KEYS                      0x0035
+
+#define BATT_SMBUS_LIFETIME_FLUSH                     0x002E
+#define BATT_SMBUS_LIFETIME_BLOCK_ONE                 0x0060
+#define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS      0x4938
+#define BATT_SMBUS_SEAL                               0x0030
+#define BATT_SMBUS_DASTATUS1                          0x0071
+#define BATT_SMBUS_DASTATUS2                          0x0072
+#define BATT_SMBUS_DASTATUS3                          0x007B
+
+#define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT      0xcf
+#define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce
 
 enum class SMBUS_DEVICE_TYPE {
-	UNDEFINED     = 0,
-	BQ40Z50       = 1,
-	BQ40Z80       = 2,
+    UNDEFINED = 0,
+    BQ40Z50   = 1,
+    BQ40Z80   = 2,
 };
 
-class BATT_SMBUS : public I2CSPIDriver<BATT_SMBUS>
-{
+class BATT_SMBUS : public I2CSPIDriver<BATT_SMBUS> {
 public:
-	BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface);
+    BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface);
 
-	~BATT_SMBUS();
+    ~BATT_SMBUS();
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	friend SMBus;
+    friend SMBus;
 
-	void RunImpl();
+    void RunImpl();
 
-	void custom_method(const BusCLIArguments &cli) override;
+    void custom_method(const BusCLIArguments &cli) override;
 
-	/**
+    /**
 	 * @brief Reads data from flash.
 	 * @param address The address to start the read from.
 	 * @param data The returned data.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int dataflash_read(const uint16_t address, void *data, const unsigned length);
+    int dataflash_read(const uint16_t address, void *data, const unsigned length);
 
-	/**
+    /**
 	 * @brief Writes data to flash.
 	 * @param address The start address of the write.
 	 * @param data The data to be written.
 	 * @param length The number of bytes being written.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int dataflash_write(const uint16_t address, void *data, const unsigned length);
+    int dataflash_write(const uint16_t address, void *data, const unsigned length);
 
-	/**
+    /**
 	* @brief Read info from battery on startup.
 	* @return Returns PX4_OK on success, PX4_ERROR on failure.
 	*/
-	int get_startup_info();
+    int get_startup_info();
 
-	/**
+    /**
 	 * @brief Performs a ManufacturerBlockAccess() read command.
 	 * @param cmd_code The command code.
 	 * @param data The returned data.
 	 * @param length The number of bytes being written.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length);
+    int manufacturer_read(const uint16_t cmd_code, void *data, const unsigned length);
 
-	/**
+    /**
 	 * @brief Performs a ManufacturerBlockAccess() write command.
 	 * @param cmd_code The command code.
 	 * @param data The sent data.
 	 * @param length The number of bytes being written.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length);
+    int manufacturer_write(const uint16_t cmd_code, void *data, const unsigned length);
 
-	/**
+    /**
 	 * @brief Unseals the battery to allow writing to restricted flash.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int unseal();
+    int unseal();
 
-	/**
+    /**
 	 * @brief Seals the battery to disallow writing to restricted flash.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int seal();
+    int seal();
 
-	/**
+    /**
 	 * @brief This command flushes the RAM Lifetime Data to data flash to help streamline evaluation testing.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int lifetime_data_flush();
+    int lifetime_data_flush();
 
-	/**
+    /**
 	 * @brief Reads the lifetime data from block 1.
 	 * @return Returns PX4_OK on success, PX4_ERROR on failure.
 	 */
-	int lifetime_read_block_one();
+    int lifetime_read_block_one();
 
-	/**
+    /**
 	 * @brief Reads the cell voltages.
 	 * @return Returns PX4_OK on success or associated read error code on failure.
 	 */
-	int get_cell_voltages();
+    int get_cell_voltages();
 
-	/**
+    /**
 	 * @brief Enables or disables the cell under voltage protection emergency shut off.
 	 */
-	void set_undervoltage_protection(float average_current);
+    void set_undervoltage_protection(float average_current);
 
-	void suspend();
+    void suspend();
 
-	void resume();
+    void resume();
 
 private:
+    SMBus *_interface;
 
-	SMBus *_interface;
-
-	SMBUS_DEVICE_TYPE _device_type{SMBUS_DEVICE_TYPE::UNDEFINED};
+    SMBUS_DEVICE_TYPE _device_type{SMBUS_DEVICE_TYPE::UNDEFINED};
 
-	perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batt_smbus_cycle")};
+    perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batt_smbus_cycle")};
 
-	static const uint8_t MAX_NUM_OF_CELLS = 7;
-	float _cell_voltages[MAX_NUM_OF_CELLS] {};
+    static const uint8_t MAX_NUM_OF_CELLS = 7;
+    float                _cell_voltages[MAX_NUM_OF_CELLS]{};
 
-	float _max_cell_voltage_delta{0};
+    float _max_cell_voltage_delta{0};
 
-	float _min_cell_voltage{0};
+    float _min_cell_voltage{0};
 
-	float _pack_power{0};
-	float _pack_average_power{0};
+    float _pack_power{0};
+    float _pack_average_power{0};
 
-	/** @param _last_report Last published report, used for test(). */
-	battery_status_s _last_report{};
+    /** @param _last_report Last published report, used for test(). */
+    battery_status_s _last_report{};
 
-	/** @param _batt_topic uORB battery topic. */
-	orb_advert_t _batt_topic{nullptr};
+    /** @param _batt_topic uORB battery topic. */
+    orb_advert_t _batt_topic{nullptr};
 
-	/** @param _cell_count Number of series cell. */
-	uint8_t _cell_count{0};
+    /** @param _cell_count Number of series cell. */
+    uint8_t _cell_count{0};
 
-	/** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */
-	uint16_t _batt_capacity{0};
+    /** @param _batt_capacity Battery design capacity in mAh (0 means unknown). */
+    uint16_t _batt_capacity{0};
 
-	/** @param _batt_startup_capacity Battery remaining capacity in mAh on startup. */
-	uint16_t _batt_startup_capacity{0};
+    /** @param _batt_startup_capacity Battery remaining capacity in mAh on startup. */
+    uint16_t _batt_startup_capacity{0};
 
-	/** @param _cycle_count The number of cycles the battery has experienced. */
-	uint16_t _cycle_count{0};
+    /** @param _cycle_count The number of cycles the battery has experienced. */
+    uint16_t _cycle_count{0};
 
-	/** @param _serial_number Serial number register. */
-	uint16_t _serial_number{0};
+    /** @param _serial_number Serial number register. */
+    uint16_t _serial_number{0};
 
-	/** @param _crit_thr Critical battery threshold param. */
-	float _crit_thr{0.f};
+    /** @param _crit_thr Critical battery threshold param. */
+    float _crit_thr{0.f};
 
-	/** @param _emergency_thr Emergency battery threshold param. */
-	float _emergency_thr{0.f};
+    /** @param _emergency_thr Emergency battery threshold param. */
+    float _emergency_thr{0.f};
 
-	/** @param _low_thr Low battery threshold param. */
-	float _low_thr{0.f};
+    /** @param _low_thr Low battery threshold param. */
+    float _low_thr{0.f};
 
-	/** @parama _c_mult Capacity/current multiplier param  */
-	float _c_mult{0.f};
+    /** @parama _c_mult Capacity/current multiplier param  */
+    float _c_mult{0.f};
 
-	/** @param _manufacturer_name Name of the battery manufacturer. */
-	char _manufacturer_name[BATT_SMBUS_MANUFACTURER_NAME_SIZE + 1] {};	// Plus one for terminator
+    /** @param _manufacturer_name Name of the battery manufacturer. */
+    char _manufacturer_name[BATT_SMBUS_MANUFACTURER_NAME_SIZE + 1]{}; // Plus one for terminator
 
-	/** @param _manufacture_date Date of the battery manufacturing. */
-	uint16_t _manufacture_date{0};
+    /** @param _manufacture_date Date of the battery manufacturing. */
+    uint16_t _manufacture_date{0};
 
-	/** @param _state_of_health state of health as read on connection  */
-	float _state_of_health{0.f};
+    /** @param _state_of_health state of health as read on connection  */
+    float _state_of_health{0.f};
 
-	/** @param _lifetime_max_delta_cell_voltage Max lifetime delta of the battery cells */
-	float _lifetime_max_delta_cell_voltage{0.f};
+    /** @param _lifetime_max_delta_cell_voltage Max lifetime delta of the battery cells */
+    float _lifetime_max_delta_cell_voltage{0.f};
 
-	/** @param _cell_undervoltage_protection_status 0 if protection disabled, 1 if enabled */
-	uint8_t _cell_undervoltage_protection_status{1};
+    /** @param _cell_undervoltage_protection_status 0 if protection disabled, 1 if enabled */
+    uint8_t _cell_undervoltage_protection_status{1};
 
-	BATT_SMBUS(const BATT_SMBUS &) = delete;
-	BATT_SMBUS operator=(const BATT_SMBUS &) = delete;
+    BATT_SMBUS(const BATT_SMBUS &)           = delete;
+    BATT_SMBUS operator=(const BATT_SMBUS &) = delete;
 };
diff --git a/apps/peripheral/battery/batt_smbus/parameters.c b/apps/peripheral/battery/batt_smbus/parameters.c
index 0d256c7577..a1e50726c0 100644
--- a/apps/peripheral/battery/batt_smbus/parameters.c
+++ b/apps/peripheral/battery/batt_smbus/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * SMBUS Smart battery driver BQ40Z50 and BQ40Z80
diff --git a/apps/peripheral/notification/lights/rgbled/rgbled.cpp b/apps/peripheral/notification/lights/rgbled/rgbled.cpp
index 1780978af4..26db543c58 100644
--- a/apps/peripheral/notification/lights/rgbled/rgbled.cpp
+++ b/apps/peripheral/notification/lights/rgbled/rgbled.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rgbled.cpp
@@ -51,263 +28,256 @@
 
 using namespace time_literals;
 
-#define ADDR			0x55	/**< I2C adress of TCA62724FMG */
-#define SUB_ADDR_START		0x01	/**< write everything (with auto-increment) */
-#define SUB_ADDR_PWM0		0x81	/**< blue     (without auto-increment) */
-#define SUB_ADDR_PWM1		0x82	/**< green    (without auto-increment) */
-#define SUB_ADDR_PWM2		0x83	/**< red      (without auto-increment) */
-#define SUB_ADDR_SETTINGS	0x84	/**< settings (without auto-increment)*/
-
-#define SETTING_NOT_POWERSAVE	0x01	/**< power-save mode not off */
-#define SETTING_ENABLE   	0x02	/**< on */
+#define ADDR                  0x55 /**< I2C adress of TCA62724FMG */
+#define SUB_ADDR_START        0x01 /**< write everything (with auto-increment) */
+#define SUB_ADDR_PWM0         0x81 /**< blue     (without auto-increment) */
+#define SUB_ADDR_PWM1         0x82 /**< green    (without auto-increment) */
+#define SUB_ADDR_PWM2         0x83 /**< red      (without auto-increment) */
+#define SUB_ADDR_SETTINGS     0x84 /**< settings (without auto-increment)*/
 
+#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
+#define SETTING_ENABLE        0x02 /**< on */
 
-class RGBLED : public device::I2C, public I2CSPIDriver<RGBLED>
-{
+class RGBLED : public device::I2C, public I2CSPIDriver<RGBLED> {
 public:
-	RGBLED(const I2CSPIDriverConfig &config);
-	virtual ~RGBLED() = default;
+    RGBLED(const I2CSPIDriverConfig &config);
+    virtual ~RGBLED() = default;
 
-	static void print_usage();
+    static void print_usage();
 
-	int		init() override;
-	int		probe() override;
+    int init() override;
+    int probe() override;
 
-	void			RunImpl();
+    void RunImpl();
 
 private:
-	void			print_status() override;
-	int			send_led_enable(bool enable);
-	int			send_led_rgb();
-	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+    void print_status() override;
+    int  send_led_enable(bool enable);
+    int  send_led_rgb();
+    int  get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
 
-	float			_brightness{1.0f};
+    float _brightness{1.0f};
 
-	uint8_t			_r{0};
-	uint8_t			_g{0};
-	uint8_t			_b{0};
-	bool			_leds_enabled{true};
+    uint8_t _r{0};
+    uint8_t _g{0};
+    uint8_t _b{0};
+    bool    _leds_enabled{true};
 
-	LedController		_led_controller;
+    LedController _led_controller;
 };
 
 RGBLED::RGBLED(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config)
-{
+    I2C(config),
+    I2CSPIDriver(config) {
 }
 
-int
-RGBLED::init()
-{
-	int ret = I2C::init();
+int RGBLED::init() {
+    int ret = I2C::init();
 
-	if (ret != OK) {
-		return ret;
-	}
+    if (ret != OK) {
+        return ret;
+    }
 
-	/* switch off LED on start */
-	send_led_enable(false);
-	send_led_rgb();
+    /* switch off LED on start */
+    send_led_enable(false);
+    send_led_rgb();
 
-	// kick off work queue
-	ScheduleNow();
+    // kick off work queue
+    ScheduleNow();
 
-	return OK;
+    return OK;
 }
 
-int
-RGBLED::probe()
-{
-	int ret;
-	bool on, powersave;
-	uint8_t r, g, b;
+int RGBLED::probe() {
+    int     ret;
+    bool    on, powersave;
+    uint8_t r, g, b;
 
-	if ((ret = get(on, powersave, r, g, b)) != OK ||
-	    (ret = send_led_enable(false) != OK) ||
-	    (ret = send_led_enable(false) != OK)) {
-		return ret;
-	}
+    if ((ret = get(on, powersave, r, g, b)) != OK || (ret = send_led_enable(false) != OK) || (ret = send_led_enable(false) != OK)) {
+        return ret;
+    }
 
-	_retries = 1;
+    _retries = 1;
 
-	return ret;
+    return ret;
 }
 
-void
-RGBLED::print_status()
-{
-	bool on, powersave;
-	uint8_t r, g, b;
+void RGBLED::print_status() {
+    bool    on, powersave;
+    uint8_t r, g, b;
 
-	int ret = get(on, powersave, r, g, b);
+    int ret = get(on, powersave, r, g, b);
 
-	if (ret == OK) {
-		/* we don't care about power-save mode */
-		PX4_INFO("state: %s", on ? "ON" : "OFF");
-		PX4_INFO("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+    if (ret == OK) {
+        /* we don't care about power-save mode */
+        PX4_INFO("state: %s", on ? "ON" : "OFF");
+        PX4_INFO("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
 
-	} else {
-		PX4_WARN("failed to read led");
-	}
+    } else {
+        PX4_WARN("failed to read led");
+    }
 }
 
-void
-RGBLED::RunImpl()
-{
-	LedControlData led_control_data;
-
-	if (_led_controller.update(led_control_data) == 1) {
-		switch (led_control_data.leds[0].color) {
-		case led_control_s::COLOR_RED:
-			_r = 255; _g = 0; _b = 0;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_GREEN:
-			_r = 0; _g = 255; _b = 0;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_BLUE:
-			_r = 0; _g = 0; _b = 255;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_AMBER: //make it the same as yellow
-		case led_control_s::COLOR_YELLOW:
-			_r = 255; _g = 255; _b = 0;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_PURPLE:
-			_r = 255; _g = 0; _b = 255;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_CYAN:
-			_r = 0; _g = 255; _b = 255;
-			send_led_enable(true);
-			break;
-
-		case led_control_s::COLOR_WHITE:
-			_r = 255; _g = 255; _b = 255;
-			send_led_enable(true);
-			break;
-
-		default: // led_control_s::COLOR_OFF
-			_r = 0; _g = 0; _b = 0;
-			send_led_enable(false);
-			break;
-		}
-
-		_brightness = (float)led_control_data.leds[0].brightness / 255.f;
-
-		send_led_rgb();
-	}
-
-	/* re-queue ourselves to run again later */
-	ScheduleDelayed(_led_controller.maximum_update_interval());
+void RGBLED::RunImpl() {
+    LedControlData led_control_data;
+
+    if (_led_controller.update(led_control_data) == 1) {
+        switch (led_control_data.leds[0].color) {
+        case led_control_s::COLOR_RED:
+            _r = 255;
+            _g = 0;
+            _b = 0;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_GREEN:
+            _r = 0;
+            _g = 255;
+            _b = 0;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_BLUE:
+            _r = 0;
+            _g = 0;
+            _b = 255;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_AMBER: //make it the same as yellow
+        case led_control_s::COLOR_YELLOW:
+            _r = 255;
+            _g = 255;
+            _b = 0;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_PURPLE:
+            _r = 255;
+            _g = 0;
+            _b = 255;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_CYAN:
+            _r = 0;
+            _g = 255;
+            _b = 255;
+            send_led_enable(true);
+            break;
+
+        case led_control_s::COLOR_WHITE:
+            _r = 255;
+            _g = 255;
+            _b = 255;
+            send_led_enable(true);
+            break;
+
+        default: // led_control_s::COLOR_OFF
+            _r = 0;
+            _g = 0;
+            _b = 0;
+            send_led_enable(false);
+            break;
+        }
+
+        _brightness = (float)led_control_data.leds[0].brightness / 255.f;
+
+        send_led_rgb();
+    }
+
+    /* re-queue ourselves to run again later */
+    ScheduleDelayed(_led_controller.maximum_update_interval());
 }
 
 /**
  * Sent ENABLE flag to LED driver
  */
-int
-RGBLED::send_led_enable(bool enable)
-{
-	if (_leds_enabled && enable) {
-		// already enabled
-		return 0;
-	}
+int RGBLED::send_led_enable(bool enable) {
+    if (_leds_enabled && enable) {
+        // already enabled
+        return 0;
+    }
 
-	_leds_enabled = enable;
-	uint8_t settings_byte = 0;
+    _leds_enabled         = enable;
+    uint8_t settings_byte = 0;
 
-	if (enable) {
-		settings_byte |= SETTING_ENABLE;
-	}
+    if (enable) {
+        settings_byte |= SETTING_ENABLE;
+    }
 
-	settings_byte |= SETTING_NOT_POWERSAVE;
+    settings_byte |= SETTING_NOT_POWERSAVE;
 
-	const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
+    const uint8_t msg[2] = {SUB_ADDR_SETTINGS, settings_byte};
 
-	return transfer(msg, sizeof(msg), nullptr, 0);
+    return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
 /**
  * Send RGB PWM settings to LED driver according to current color and brightness
  */
-int
-RGBLED::send_led_rgb()
-{
-	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
-	const uint8_t msg[6] = {
-		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness + 0.5f),
-		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness + 0.5f),
-		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness + 0.5f)
-	};
-	return transfer(msg, sizeof(msg), nullptr, 0);
+int RGBLED::send_led_rgb() {
+    /* To scale from 0..255 -> 0..15 shift right by 4 bits */
+    const uint8_t msg[6] = {
+        SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness + 0.5f),
+        SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness + 0.5f),
+        SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness + 0.5f)};
+    return transfer(msg, sizeof(msg), nullptr, 0);
 }
 
-int
-RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
-{
-	uint8_t result[2] = {0, 0};
-	int ret;
+int RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) {
+    uint8_t result[2] = {0, 0};
+    int     ret;
 
-	ret = transfer(nullptr, 0, &result[0], 2);
+    ret = transfer(nullptr, 0, &result[0], 2);
 
-	if (ret == OK) {
-		on = ((result[0] >> 4) & SETTING_ENABLE);
-		powersave = !((result[0] >> 4) & SETTING_NOT_POWERSAVE);
-		/* XXX check, looks wrong */
-		r = (result[0] & 0x0f) << 4;
-		g = (result[1] & 0xf0);
-		b = (result[1] & 0x0f) << 4;
-	}
+    if (ret == OK) {
+        on        = ((result[0] >> 4) & SETTING_ENABLE);
+        powersave = !((result[0] >> 4) & SETTING_NOT_POWERSAVE);
+        /* XXX check, looks wrong */
+        r = (result[0] & 0x0f) << 4;
+        g = (result[1] & 0xf0);
+        b = (result[1] & 0x0f) << 4;
+    }
 
-	return ret;
+    return ret;
 }
 
-void
-RGBLED::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("rgbled", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x55);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void RGBLED::print_usage() {
+    PRINT_MODULE_USAGE_NAME("rgbled", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x55);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" __EXPORT int rgbled_main(int argc, char *argv[])
-{
-	using ThisDriver = RGBLED;
-	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = 100000;
-	cli.i2c_address = ADDR;
+extern "C" __EXPORT int rgbled_main(int argc, char *argv[]) {
+    using ThisDriver = RGBLED;
+    BusCLIArguments cli{true, false};
+    cli.default_i2c_frequency = 100000;
+    cli.i2c_address           = ADDR;
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+    const char *verb = cli.parseDefaultArguments(argc, argv);
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/notification/lights/rgbled_pwm/rgbled_pwm.cpp b/apps/peripheral/notification/lights/rgbled_pwm/rgbled_pwm.cpp
index 3e33ee0e13..51f21c0d9f 100644
--- a/apps/peripheral/notification/lights/rgbled_pwm/rgbled_pwm.cpp
+++ b/apps/peripheral/notification/lights/rgbled_pwm/rgbled_pwm.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015, 2016 Airmind 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 Airmind 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rgbled_pwm.cpp
@@ -45,262 +22,260 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-class RGBLED_PWM : public px4::ScheduledWorkItem
-{
+class RGBLED_PWM : public px4::ScheduledWorkItem {
 public:
-	RGBLED_PWM();
-	virtual ~RGBLED_PWM();
+    RGBLED_PWM();
+    virtual ~RGBLED_PWM();
 
-	int			init();
-	int			status();
+    int init();
+    int status();
 
 private:
+    uint8_t _r{0};
+    uint8_t _g{0};
+    uint8_t _b{0};
 
-	uint8_t			_r{0};
-	uint8_t			_g{0};
-	uint8_t			_b{0};
-
-	volatile bool		_running{false};
-	volatile bool		_should_run{true};
+    volatile bool _running{false};
+    volatile bool _should_run{true};
 
-	LedController		_led_controller;
+    LedController _led_controller;
 
-	void			Run() override;
+    void Run() override;
 
-	int			send_led_rgb();
-	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+    int send_led_rgb();
+    int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
 };
 
-extern int led_pwm_servo_set(unsigned channel, uint8_t  value);
-extern int led_pwm_servo_set_ex(uint8_t r, uint8_t g, uint8_t b, uint8_t led_count);
+extern int      led_pwm_servo_set(unsigned channel, uint8_t value);
+extern int      led_pwm_servo_set_ex(uint8_t r, uint8_t g, uint8_t b, uint8_t led_count);
 extern unsigned led_pwm_servo_get(unsigned channel);
-extern int led_pwm_servo_init(void);
-extern void led_pwm_servo_deinit(void);
-
+extern int      led_pwm_servo_init(void);
+extern void     led_pwm_servo_deinit(void);
 
 /* for now, we only support one RGBLED */
-namespace
-{
+namespace {
 RGBLED_PWM *g_rgbled = nullptr;
 }
 
 RGBLED_PWM::RGBLED_PWM() :
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
+    ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) {
 }
 
-RGBLED_PWM::~RGBLED_PWM()
-{
-	_should_run = false;
-	int counter = 0;
+RGBLED_PWM::~RGBLED_PWM() {
+    _should_run = false;
+    int counter = 0;
 
-	while (_running && ++counter < 10) {
-		px4_usleep(100000);
-	}
+    while (_running && ++counter < 10) {
+        px4_usleep(100000);
+    }
 }
 
-int
-RGBLED_PWM::init()
-{
-	/* switch off LED on start */
-	led_pwm_servo_init();
-	send_led_rgb();
+int RGBLED_PWM::init() {
+    /* switch off LED on start */
+    led_pwm_servo_init();
+    send_led_rgb();
 
-	_running = true;
+    _running = true;
 
-	// kick off work queue
-	ScheduleNow();
+    // kick off work queue
+    ScheduleNow();
 
-	return OK;
+    return OK;
 }
 
-int
-RGBLED_PWM::status()
-{
-	bool on, powersave;
-	uint8_t r, g, b;
+int RGBLED_PWM::status() {
+    bool    on, powersave;
+    uint8_t r, g, b;
 
-	int ret = get(on, powersave, r, g, b);
+    int ret = get(on, powersave, r, g, b);
 
-	if (ret == OK) {
-		/* we don't care about power-save mode */
-		PX4_INFO("state: %s", on ? "ON" : "OFF");
-		PX4_INFO("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+    if (ret == OK) {
+        /* we don't care about power-save mode */
+        PX4_INFO("state: %s", on ? "ON" : "OFF");
+        PX4_INFO("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
 
-	} else {
-		PX4_WARN("failed to read led");
-	}
+    } else {
+        PX4_WARN("failed to read led");
+    }
 
-	return ret;
+    return ret;
 }
 
 /**
  * Main loop function
  */
-void
-RGBLED_PWM::Run()
-{
-	if (!_should_run) {
-		_running = false;
-		return;
-	}
-
-	LedControlData led_control_data;
-
-	if (_led_controller.update(led_control_data) == 1) {
-		uint8_t brightness = led_control_data.leds[0].brightness;
-
-		switch (led_control_data.leds[0].color) {
-		case led_control_s::COLOR_RED:
-			_r = brightness; _g = 0; _b = 0;
-			break;
-
-		case led_control_s::COLOR_GREEN:
-			_r = 0; _g = brightness; _b = 0;
-			break;
-
-		case led_control_s::COLOR_BLUE:
-			_r = 0; _g = 0; _b = brightness;
-			break;
-
-		case led_control_s::COLOR_AMBER: //make it the same as yellow
-		case led_control_s::COLOR_YELLOW:
-			_r = brightness; _g = brightness; _b = 0;
-			break;
-
-		case led_control_s::COLOR_PURPLE:
-			_r = brightness; _g = 0; _b = brightness;
-			break;
-
-		case led_control_s::COLOR_CYAN:
-			_r = 0; _g = brightness; _b = brightness;
-			break;
-
-		case led_control_s::COLOR_WHITE:
-			_r = brightness; _g = brightness; _b = brightness;
-			break;
-
-		default: // led_control_s::COLOR_OFF
-			_r = 0; _g = 0; _b = 0;
-			break;
-		}
-
-		send_led_rgb();
-	}
-
-	/* re-queue ourselves to run again later */
-	ScheduleDelayed(_led_controller.maximum_update_interval());
+void RGBLED_PWM::Run() {
+    if (!_should_run) {
+        _running = false;
+        return;
+    }
+
+    LedControlData led_control_data;
+
+    if (_led_controller.update(led_control_data) == 1) {
+        uint8_t brightness = led_control_data.leds[0].brightness;
+
+        switch (led_control_data.leds[0].color) {
+        case led_control_s::COLOR_RED:
+            _r = brightness;
+            _g = 0;
+            _b = 0;
+            break;
+
+        case led_control_s::COLOR_GREEN:
+            _r = 0;
+            _g = brightness;
+            _b = 0;
+            break;
+
+        case led_control_s::COLOR_BLUE:
+            _r = 0;
+            _g = 0;
+            _b = brightness;
+            break;
+
+        case led_control_s::COLOR_AMBER: //make it the same as yellow
+        case led_control_s::COLOR_YELLOW:
+            _r = brightness;
+            _g = brightness;
+            _b = 0;
+            break;
+
+        case led_control_s::COLOR_PURPLE:
+            _r = brightness;
+            _g = 0;
+            _b = brightness;
+            break;
+
+        case led_control_s::COLOR_CYAN:
+            _r = 0;
+            _g = brightness;
+            _b = brightness;
+            break;
+
+        case led_control_s::COLOR_WHITE:
+            _r = brightness;
+            _g = brightness;
+            _b = brightness;
+            break;
+
+        default: // led_control_s::COLOR_OFF
+            _r = 0;
+            _g = 0;
+            _b = 0;
+            break;
+        }
+
+        send_led_rgb();
+    }
+
+    /* re-queue ourselves to run again later */
+    ScheduleDelayed(_led_controller.maximum_update_interval());
 }
 
 /**
  * Send RGB PWM settings to LED driver according to current color and brightness
  */
-int
-RGBLED_PWM::send_led_rgb()
-{
+int RGBLED_PWM::send_led_rgb() {
 #if defined(BOARD_HAS_LED_PWM)
-	led_pwm_servo_set(0, _r);
-	led_pwm_servo_set(1, _g);
-	led_pwm_servo_set(2, _b);
+    led_pwm_servo_set(0, _r);
+    led_pwm_servo_set(1, _g);
+    led_pwm_servo_set(2, _b);
 #endif
 
 #if defined(BOARD_HAS_UI_LED_PWM)
-	led_pwm_servo_set(3, _r);
-	led_pwm_servo_set(4, _g);
-	led_pwm_servo_set(5, _b);
+    led_pwm_servo_set(3, _r);
+    led_pwm_servo_set(4, _g);
+    led_pwm_servo_set(5, _b);
 #endif
-	return (OK);
+    return (OK);
 }
 
-int
-RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
-{
-	powersave = OK;
-	on = _r > 0 || _g > 0 || _b > 0;
-	r = _r;
-	g = _g;
-	b = _b;
-	return OK;
+int RGBLED_PWM::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) {
+    powersave = OK;
+    on        = _r > 0 || _g > 0 || _b > 0;
+    r         = _r;
+    g         = _g;
+    b         = _b;
+    return OK;
 }
 
 static void
-rgbled_usage()
-{
-	PX4_INFO("missing command: try 'start', 'status', 'stop'");
+rgbled_usage() {
+    PX4_INFO("missing command: try 'start', 'status', 'stop'");
 }
 
 extern "C" __EXPORT int
-rgbled_pwm_main(int argc, char *argv[])
-{
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-
-	/* jump over start/off/etc and look at options first */
-	while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'a':
-			break;
-
-		case 'b':
-			break;
-
-		default:
-			rgbled_usage();
-			return 1;
-		}
-	}
-
-	if (myoptind >= argc) {
-		rgbled_usage();
-		return 1;
-	}
-
-	const char *verb = argv[myoptind];
-
-	if (!strcmp(verb, "start")) {
-		if (g_rgbled != nullptr) {
-			PX4_WARN("already started");
-			return 1;
-		}
-
-		if (g_rgbled == nullptr) {
-			g_rgbled = new RGBLED_PWM();
-
-			if (g_rgbled == nullptr) {
-				PX4_WARN("alloc failed");
-				return 1;
-			}
-
-			if (OK != g_rgbled->init()) {
-				delete g_rgbled;
-				g_rgbled = nullptr;
-				PX4_ERR("init failed");
-				return 1;
-			}
-		}
-
-		return 0;
-	}
-
-	/* need the driver past this point */
-	if (g_rgbled == nullptr) {
-		PX4_WARN("not started");
-		rgbled_usage();
-		return 1;
-	}
-
-	if (!strcmp(verb, "status")) {
-		g_rgbled->status();
-		return 0;
-	}
-
-	if (!strcmp(verb, "stop")) {
-		delete g_rgbled;
-		g_rgbled = nullptr;
-		return 0;
-	}
-
-	rgbled_usage();
-	return 1;
+rgbled_pwm_main(int argc, char *argv[]) {
+    int         myoptind = 1;
+    int         ch;
+    const char *myoptarg = nullptr;
+
+    /* jump over start/off/etc and look at options first */
+    while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) {
+        switch (ch) {
+        case 'a':
+            break;
+
+        case 'b':
+            break;
+
+        default:
+            rgbled_usage();
+            return 1;
+        }
+    }
+
+    if (myoptind >= argc) {
+        rgbled_usage();
+        return 1;
+    }
+
+    const char *verb = argv[myoptind];
+
+    if (!strcmp(verb, "start")) {
+        if (g_rgbled != nullptr) {
+            PX4_WARN("already started");
+            return 1;
+        }
+
+        if (g_rgbled == nullptr) {
+            g_rgbled = new RGBLED_PWM();
+
+            if (g_rgbled == nullptr) {
+                PX4_WARN("alloc failed");
+                return 1;
+            }
+
+            if (OK != g_rgbled->init()) {
+                delete g_rgbled;
+                g_rgbled = nullptr;
+                PX4_ERR("init failed");
+                return 1;
+            }
+        }
+
+        return 0;
+    }
+
+    /* need the driver past this point */
+    if (g_rgbled == nullptr) {
+        PX4_WARN("not started");
+        rgbled_usage();
+        return 1;
+    }
+
+    if (!strcmp(verb, "status")) {
+        g_rgbled->status();
+        return 0;
+    }
+
+    if (!strcmp(verb, "stop")) {
+        delete g_rgbled;
+        g_rgbled = nullptr;
+        return 0;
+    }
+
+    rgbled_usage();
+    return 1;
 }
diff --git a/apps/peripheral/payload/camera_capture/camera_capture.cpp b/apps/peripheral/payload/camera_capture/camera_capture.cpp
index 91ab266311..aca4b52f41 100644
--- a/apps/peripheral/payload/camera_capture/camera_capture.cpp
+++ b/apps/peripheral/payload/camera_capture/camera_capture.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_capture.cpp
@@ -49,427 +26,391 @@
 
 using namespace time_literals;
 
-namespace camera_capture
-{
+namespace camera_capture {
 CameraCapture *g_camera_capture{nullptr};
 }
 
 struct work_s CameraCapture::_work_publisher;
 
 CameraCapture::CameraCapture() :
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-	memset(&_work_publisher, 0, sizeof(_work_publisher));
+    ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) {
+    memset(&_work_publisher, 0, sizeof(_work_publisher));
 
-	// Capture Parameters
-	_p_strobe_delay = param_find("CAM_CAP_DELAY");
-	param_get(_p_strobe_delay, &_strobe_delay);
+    // Capture Parameters
+    _p_strobe_delay = param_find("CAM_CAP_DELAY");
+    param_get(_p_strobe_delay, &_strobe_delay);
 
-	_p_camera_capture_mode = param_find("CAM_CAP_MODE");
-	param_get(_p_camera_capture_mode, &_camera_capture_mode);
+    _p_camera_capture_mode = param_find("CAM_CAP_MODE");
+    param_get(_p_camera_capture_mode, &_camera_capture_mode);
 
-	_p_camera_capture_edge = param_find("CAM_CAP_EDGE");
-	param_get(_p_camera_capture_edge, &_camera_capture_edge);
+    _p_camera_capture_edge = param_find("CAM_CAP_EDGE");
+    param_get(_p_camera_capture_edge, &_camera_capture_edge);
 
-	// get the capture channel from function configuration params
-	_capture_channel = -1;
+    // get the capture channel from function configuration params
+    _capture_channel = -1;
 
-	for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) {
-		char param_name[17];
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
-		param_t function_handle = param_find(param_name);
-		int32_t function;
+    for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) {
+        char param_name[17];
+        snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+        param_t function_handle = param_find(param_name);
+        int32_t function;
 
-		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
-			if (function == 2032) { // Camera_Capture
-				_capture_channel = i;
-			}
-		}
-	}
+        if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+            if (function == 2032) { // Camera_Capture
+                _capture_channel = i;
+            }
+        }
+    }
 
-	_trigger_pub.advertise();
+    _trigger_pub.advertise();
 }
 
-CameraCapture::~CameraCapture()
-{
-	camera_capture::g_camera_capture = nullptr;
+CameraCapture::~CameraCapture() {
+    camera_capture::g_camera_capture = nullptr;
 }
 
-void
-CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
-{
-	// Maximum acceptable rate is 5kHz
-	if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
-		++_trigger_rate_exceeded_counter;
-
-		if (_trigger_rate_exceeded_counter > 100) {
+void CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) {
+    // Maximum acceptable rate is 5kHz
+    if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
+        ++_trigger_rate_exceeded_counter;
 
-			// Trigger rate too high, stop future interrupts
-			up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
-			_trigger_rate_failure.store(true);
-		}
+        if (_trigger_rate_exceeded_counter > 100) {
+            // Trigger rate too high, stop future interrupts
+            up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
+            _trigger_rate_failure.store(true);
+        }
 
-	} else if (_trigger_rate_exceeded_counter > 0) {
-		--_trigger_rate_exceeded_counter;
-	}
+    } else if (_trigger_rate_exceeded_counter > 0) {
+        --_trigger_rate_exceeded_counter;
+    }
 
-	_trigger.chan_index = chan_index;
-	_trigger.hrt_edge_time = edge_time;
-	_trigger.edge_state = edge_state;
-	_trigger.overflow = overflow;
+    _trigger.chan_index    = chan_index;
+    _trigger.hrt_edge_time = edge_time;
+    _trigger.edge_state    = edge_state;
+    _trigger.overflow      = overflow;
 
-	work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, this, 0);
+    work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, this, 0);
 }
 
-int
-CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
-{
-	CameraCapture *dev = static_cast<CameraCapture *>(arg);
+int CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) {
+    CameraCapture *dev = static_cast<CameraCapture *>(arg);
 
-	dev->_trigger.chan_index = 0;
-	dev->_trigger.hrt_edge_time = hrt_absolute_time();
-	dev->_trigger.edge_state = 0;
-	dev->_trigger.overflow = 0;
+    dev->_trigger.chan_index    = 0;
+    dev->_trigger.hrt_edge_time = hrt_absolute_time();
+    dev->_trigger.edge_state    = 0;
+    dev->_trigger.overflow      = 0;
 
-	work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0);
+    work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0);
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-void
-CameraCapture::publish_trigger_trampoline(void *arg)
-{
-	CameraCapture *dev = static_cast<CameraCapture *>(arg);
+void CameraCapture::publish_trigger_trampoline(void *arg) {
+    CameraCapture *dev = static_cast<CameraCapture *>(arg);
 
-	dev->publish_trigger();
+    dev->publish_trigger();
 }
 
-void
-CameraCapture::publish_trigger()
-{
-	bool publish = false;
-
-	if (_trigger_rate_failure.load()) {
-		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Camera capture disabled\t");
-		events::send(events::ID("camera_capture_trigger_rate_exceeded"),
-			     events::Log::Error, "Hardware fault: Camera capture disabled");
-		_trigger_rate_failure.store(false);
-	}
-
-	camera_trigger_s trigger{};
-
-	// MODES 1 and 2 are not fully tested
-	if (_camera_capture_mode == 0 || _gpio_capture) {
-		trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
-		trigger.seq = _capture_seq++;
-		_last_trig_time = trigger.timestamp;
-
-		publish = true;
-
-	} else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
-		if (_trigger.edge_state == 1) {
-			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
-
-		} else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) {
-			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
-			trigger.seq = _capture_seq++;
-			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
-			_last_trig_time = trigger.timestamp;
-			publish = true;
-			_capture_seq++;
-		}
-
-	} else { // Get timestamp of mid-exposure (active low)
-		if (_trigger.edge_state == 0) {
-			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
-
-		} else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) {
-			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
-			trigger.seq = _capture_seq++;
-			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
-			_last_trig_time = trigger.timestamp;
-			publish = true;
-		}
-
-	}
-
-	trigger.feedback = true;
-	_capture_overflows = _trigger.overflow;
-
-	if (!publish) {
-		return;
-	}
-
-	pps_capture_s pps_capture;
-
-	if (_pps_capture_sub.update(&pps_capture)) {
-		_pps_hrt_timestamp = pps_capture.timestamp;
-		_pps_rtc_timestamp = pps_capture.rtc_timestamp;
-	}
-
-
-	if (_pps_hrt_timestamp > 0) {
-		// Last PPS RTC time + elapsed time to the camera capture interrupt
-		trigger.timestamp_utc = _pps_rtc_timestamp + (trigger.timestamp - _pps_hrt_timestamp);
-
-	} else {
-		// No PPS capture received, use RTC clock as fallback
-		timespec tv{};
-		px4_clock_gettime(CLOCK_REALTIME, &tv);
-		trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp);
-	}
-
-	_trigger_pub.publish(trigger);
+void CameraCapture::publish_trigger() {
+    bool publish = false;
+
+    if (_trigger_rate_failure.load()) {
+        mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Camera capture disabled\t");
+        events::send(events::ID("camera_capture_trigger_rate_exceeded"),
+                     events::Log::Error, "Hardware fault: Camera capture disabled");
+        _trigger_rate_failure.store(false);
+    }
+
+    camera_trigger_s trigger{};
+
+    // MODES 1 and 2 are not fully tested
+    if (_camera_capture_mode == 0 || _gpio_capture) {
+        trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+        trigger.seq       = _capture_seq++;
+        _last_trig_time   = trigger.timestamp;
+
+        publish = true;
+
+    } else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
+        if (_trigger.edge_state == 1) {
+            _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+        } else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) {
+            trigger.timestamp   = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+            trigger.seq         = _capture_seq++;
+            _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+            _last_trig_time     = trigger.timestamp;
+            publish             = true;
+            _capture_seq++;
+        }
+
+    } else { // Get timestamp of mid-exposure (active low)
+        if (_trigger.edge_state == 0) {
+            _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+        } else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) {
+            trigger.timestamp   = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+            trigger.seq         = _capture_seq++;
+            _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+            _last_trig_time     = trigger.timestamp;
+            publish             = true;
+        }
+    }
+
+    trigger.feedback   = true;
+    _capture_overflows = _trigger.overflow;
+
+    if (!publish) {
+        return;
+    }
+
+    pps_capture_s pps_capture;
+
+    if (_pps_capture_sub.update(&pps_capture)) {
+        _pps_hrt_timestamp = pps_capture.timestamp;
+        _pps_rtc_timestamp = pps_capture.rtc_timestamp;
+    }
+
+
+    if (_pps_hrt_timestamp > 0) {
+        // Last PPS RTC time + elapsed time to the camera capture interrupt
+        trigger.timestamp_utc = _pps_rtc_timestamp + (trigger.timestamp - _pps_hrt_timestamp);
+
+    } else {
+        // No PPS capture received, use RTC clock as fallback
+        timespec tv{};
+        px4_clock_gettime(CLOCK_REALTIME, &tv);
+        trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp);
+    }
+
+    _trigger_pub.publish(trigger);
 }
 
-void
-CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
-				  uint32_t overflow)
-{
-	camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
+void CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+                                       uint32_t overflow) {
+    camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
 }
 
-void
-CameraCapture::Run()
-{
-	// Command handling
-	vehicle_command_s cmd{};
+void CameraCapture::Run() {
+    // Command handling
+    vehicle_command_s cmd{};
+
+    if (_command_sub.update(&cmd)) {
+        // TODO : this should eventuallly be a capture control command
+        if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
+            // Enable/disable signal capture
+            if (commandParamToInt(cmd.param1) == 1) {
+                set_capture_control(true);
+
+            } else if (commandParamToInt(cmd.param1) == 0) {
+                set_capture_control(false);
+            }
+
+            // Reset capture sequence
+            if (commandParamToInt(cmd.param2) == 1) {
+                reset_statistics(true);
+            }
+
+            // Acknowledge the command
+            vehicle_command_ack_s command_ack{};
+
+            command_ack.timestamp        = hrt_absolute_time();
+            command_ack.command          = cmd.command;
+            command_ack.result           = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+            command_ack.target_system    = cmd.source_system;
+            command_ack.target_component = cmd.source_component;
+
+            _command_ack_pub.publish(command_ack);
+        }
+    }
+}
 
-	if (_command_sub.update(&cmd)) {
+void CameraCapture::set_capture_control(bool enabled) {
+// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin. It's used if no channel is configured
+#if defined(BOARD_CAPTURE_GPIO)
 
-		// TODO : this should eventuallly be a capture control command
-		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
+    if (_capture_channel == -1) {
+        px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &CameraCapture::gpio_interrupt_routine, this);
+        _capture_enabled = enabled;
+        _gpio_capture    = true;
+        reset_statistics(false);
+    }
 
-			// Enable/disable signal capture
-			if (commandParamToInt(cmd.param1) == 1) {
-				set_capture_control(true);
+#endif
 
-			} else if (commandParamToInt(cmd.param1) == 0) {
-				set_capture_control(false);
+    if (!_gpio_capture) {
+        if (_capture_channel == -1) {
+            PX4_WARN("No capture channel configured");
+            _capture_enabled = false;
 
-			}
+        } else {
+            capture_callback_t callback = nullptr;
+            void              *context  = nullptr;
 
-			// Reset capture sequence
-			if (commandParamToInt(cmd.param2) == 1) {
-				reset_statistics(true);
+            if (enabled) {
+                callback = &CameraCapture::capture_trampoline;
+                context  = this;
+            }
 
-			}
+            int ret = up_input_capture_set_callback(_capture_channel, callback, context);
 
-			// Acknowledge the command
-			vehicle_command_ack_s command_ack{};
+            if (ret == 0) {
+                _capture_enabled = enabled;
+                _gpio_capture    = false;
 
-			command_ack.timestamp = hrt_absolute_time();
-			command_ack.command = cmd.command;
-			command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-			command_ack.target_system = cmd.source_system;
-			command_ack.target_component = cmd.source_component;
+            } else {
+                PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", _capture_channel, ret);
+                _capture_enabled = false;
+            }
 
-			_command_ack_pub.publish(command_ack);
-		}
-	}
+            reset_statistics(false);
+        }
+    }
 }
 
-void
-CameraCapture::set_capture_control(bool enabled)
-{
-// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin. It's used if no channel is configured
-#if defined(BOARD_CAPTURE_GPIO)
-
-	if (_capture_channel == -1) {
-		px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &CameraCapture::gpio_interrupt_routine, this);
-		_capture_enabled = enabled;
-		_gpio_capture = true;
-		reset_statistics(false);
-	}
+void CameraCapture::reset_statistics(bool reset_seq) {
+    if (reset_seq) {
+        _capture_seq = 0;
+    }
 
-#endif
+    _last_trig_begin_time = 0;
+    _last_exposure_time   = 0;
+    _last_trig_time       = 0;
+    _capture_overflows    = 0;
+}
 
-	if (!_gpio_capture) {
-		if (_capture_channel == -1) {
-			PX4_WARN("No capture channel configured");
-			_capture_enabled = false;
+int CameraCapture::start() {
+    if (!_gpio_capture && _capture_channel != -1) {
+        input_capture_edge edge = Both;
 
-		} else {
-			capture_callback_t callback = nullptr;
-			void *context = nullptr;
+        if (_camera_capture_mode == 0) {
+            edge = _camera_capture_edge ? Rising : Falling;
+        }
 
-			if (enabled) {
-				callback = &CameraCapture::capture_trampoline;
-				context = this;
-			}
+        int ret = up_input_capture_set(_capture_channel, edge, 0, nullptr, nullptr);
 
-			int ret = up_input_capture_set_callback(_capture_channel, callback, context);
+        if (ret != 0) {
+            PX4_ERR("up_input_capture_set failed (%i)", ret);
+            return ret;
+        }
+    }
 
-			if (ret == 0) {
-				_capture_enabled = enabled;
-				_gpio_capture = false;
+    // run every 100 ms (10 Hz)
+    ScheduleOnInterval(100000, 10000);
 
-			} else {
-				PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", _capture_channel, ret);
-				_capture_enabled = false;
-			}
+    return PX4_OK;
+}
 
-			reset_statistics(false);
-		}
-	}
+void CameraCapture::stop() {
+    ScheduleClear();
 
-}
+    work_cancel(HPWORK, &_work_publisher);
 
-void
-CameraCapture::reset_statistics(bool reset_seq)
-{
-	if (reset_seq) {
-		_capture_seq = 0;
-	}
-
-	_last_trig_begin_time = 0;
-	_last_exposure_time = 0;
-	_last_trig_time = 0;
-	_capture_overflows = 0;
+    if (camera_capture::g_camera_capture != nullptr) {
+        delete (camera_capture::g_camera_capture);
+    }
 }
 
-int
-CameraCapture::start()
-{
-	if (!_gpio_capture && _capture_channel != -1) {
-		input_capture_edge edge = Both;
+void CameraCapture::status() {
+    PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
+    PX4_INFO("Frame sequence : %" PRIu32, _capture_seq);
 
-		if (_camera_capture_mode == 0) {
-			edge = _camera_capture_edge ? Rising : Falling;
-		}
+    if (_last_trig_time != 0) {
+        PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time,
+                 (int)(hrt_elapsed_time(&_last_trig_time) / 1000));
 
-		int ret = up_input_capture_set(_capture_channel, edge, 0, nullptr, nullptr);
+    } else {
+        PX4_INFO("No trigger yet");
+    }
 
-		if (ret != 0) {
-			PX4_ERR("up_input_capture_set failed (%i)", ret);
-			return ret;
-		}
-	}
+    if (_camera_capture_mode != 0) {
+        PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);
+    }
 
-	// run every 100 ms (10 Hz)
-	ScheduleOnInterval(100000, 10000);
+    PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
 
-	return PX4_OK;
-}
+    if (_gpio_capture) {
+        PX4_INFO("Using board GPIO pin");
 
-void
-CameraCapture::stop()
-{
-	ScheduleClear();
+    } else if (_capture_channel == -1) {
+        PX4_INFO("No Capture channel configured");
 
-	work_cancel(HPWORK, &_work_publisher);
+    } else {
+        input_capture_stats_t stats;
+        int                   ret = up_input_capture_get_stats(_capture_channel, &stats, false);
 
-	if (camera_capture::g_camera_capture != nullptr) {
-		delete (camera_capture::g_camera_capture);
-	}
-}
+        if (ret != 0) {
+            PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", _capture_channel, ret);
 
-void
-CameraCapture::status()
-{
-	PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
-	PX4_INFO("Frame sequence : %" PRIu32, _capture_seq);
-
-	if (_last_trig_time != 0) {
-		PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time,
-			 (int)(hrt_elapsed_time(&_last_trig_time) / 1000));
-
-	} else {
-		PX4_INFO("No trigger yet");
-	}
-
-	if (_camera_capture_mode != 0) {
-		PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);
-	}
-
-	PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
-
-	if (_gpio_capture) {
-		PX4_INFO("Using board GPIO pin");
-
-	} else if (_capture_channel == -1) {
-		PX4_INFO("No Capture channel configured");
-
-	} else {
-		input_capture_stats_t stats;
-		int ret =  up_input_capture_get_stats(_capture_channel, &stats, false);
-
-		if (ret != 0) {
-			PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", _capture_channel, ret);
-
-		} else {
-			PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32
-				 " overflows: %" PRIu32 " latency: %" PRIu16,
-				 _capture_channel,
-				 stats.edges,
-				 stats.last_time,
-				 stats.last_edge,
-				 stats.overflows,
-				 stats.latency);
-		}
-	}
+        } else {
+            PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32
+                     " overflows: %" PRIu32 " latency: %" PRIu16,
+                     _capture_channel,
+                     stats.edges,
+                     stats.last_time,
+                     stats.last_edge,
+                     stats.overflows,
+                     stats.latency);
+        }
+    }
 }
 
-static int usage()
-{
-	PX4_INFO("usage: camera_capture {start|stop|on|off|reset|status}\n");
-	return 1;
+static int usage() {
+    PX4_INFO("usage: camera_capture {start|stop|on|off|reset|status}\n");
+    return 1;
 }
 
 extern "C" __EXPORT int camera_capture_main(int argc, char *argv[]);
 
-int camera_capture_main(int argc, char *argv[])
-{
-	if (argc < 2) {
-		return usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (camera_capture::g_camera_capture != nullptr) {
-			PX4_WARN("already running");
-			return 0;
-		}
+int camera_capture_main(int argc, char *argv[]) {
+    if (argc < 2) {
+        return usage();
+    }
 
-		camera_capture::g_camera_capture = new CameraCapture();
+    if (!strcmp(argv[1], "start")) {
+        if (camera_capture::g_camera_capture != nullptr) {
+            PX4_WARN("already running");
+            return 0;
+        }
 
-		if (camera_capture::g_camera_capture == nullptr) {
-			PX4_WARN("alloc failed");
-			return 1;
-		}
+        camera_capture::g_camera_capture = new CameraCapture();
 
-		if (!camera_capture::g_camera_capture->start()) {
-			return 0;
+        if (camera_capture::g_camera_capture == nullptr) {
+            PX4_WARN("alloc failed");
+            return 1;
+        }
 
-		} else {
-			return 1;
-		}
+        if (!camera_capture::g_camera_capture->start()) {
+            return 0;
 
-	}
+        } else {
+            return 1;
+        }
+    }
 
-	if (camera_capture::g_camera_capture == nullptr) {
-		PX4_WARN("not running");
-		return 1;
+    if (camera_capture::g_camera_capture == nullptr) {
+        PX4_WARN("not running");
+        return 1;
 
-	} else if (!strcmp(argv[1], "stop")) {
-		camera_capture::g_camera_capture->stop();
+    } else if (!strcmp(argv[1], "stop")) {
+        camera_capture::g_camera_capture->stop();
 
-	} else if (!strcmp(argv[1], "status")) {
-		camera_capture::g_camera_capture->status();
+    } else if (!strcmp(argv[1], "status")) {
+        camera_capture::g_camera_capture->status();
 
-	} else if (!strcmp(argv[1], "on")) {
-		camera_capture::g_camera_capture->set_capture_control(true);
+    } else if (!strcmp(argv[1], "on")) {
+        camera_capture::g_camera_capture->set_capture_control(true);
 
-	} else if (!strcmp(argv[1], "off")) {
-		camera_capture::g_camera_capture->set_capture_control(false);
+    } else if (!strcmp(argv[1], "off")) {
+        camera_capture::g_camera_capture->set_capture_control(false);
 
-	} else if (!strcmp(argv[1], "reset")) {
-		camera_capture::g_camera_capture->set_capture_control(false);
-		camera_capture::g_camera_capture->reset_statistics(true);
+    } else if (!strcmp(argv[1], "reset")) {
+        camera_capture::g_camera_capture->set_capture_control(false);
+        camera_capture::g_camera_capture->reset_statistics(true);
 
-	} else {
-		return usage();
-	}
+    } else {
+        return usage();
+    }
 
-	return 0;
+    return 0;
 }
diff --git a/apps/peripheral/payload/camera_capture/camera_capture.hpp b/apps/peripheral/payload/camera_capture/camera_capture.hpp
index 92dd2790d1..fc26ba7198 100644
--- a/apps/peripheral/payload/camera_capture/camera_capture.hpp
+++ b/apps/peripheral/payload/camera_capture/camera_capture.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_capture.hpp
@@ -55,97 +32,95 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
-class CameraCapture : public px4::ScheduledWorkItem
-{
+class CameraCapture : public px4::ScheduledWorkItem {
 public:
-	/**
+    /**
 	 * Constructor
 	 */
-	CameraCapture();
+    CameraCapture();
 
-	/**
+    /**
 	 * Destructor, also kills task.
 	 */
-	~CameraCapture();
+    ~CameraCapture();
 
-	/**
+    /**
 	 * Start the task.
 	 */
-	int			start();
+    int start();
 
-	/**
+    /**
 	 * Stop the task.
 	 */
-	void			stop();
+    void stop();
 
-	void 			status();
+    void status();
 
-	// Low-rate command handling loop
-	void			Run() override;
+    // Low-rate command handling loop
+    void Run() override;
 
-	static void		capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
-			uint32_t overflow);
+    static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+                                   uint32_t overflow);
 
-	void 			set_capture_control(bool enabled);
+    void set_capture_control(bool enabled);
 
-	void			reset_statistics(bool reset_seq);
+    void reset_statistics(bool reset_seq);
 
-	void			publish_trigger();
+    void publish_trigger();
 
 
-	static struct work_s	_work_publisher;
+    static struct work_s _work_publisher;
 
 private:
-	int _capture_channel = 5; ///< by default, use FMU output 6
-
-	// Publishers
-	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::Publication<camera_trigger_s>		_trigger_pub{ORB_ID(camera_trigger)};
-
-	// Subscribers
-	uORB::Subscription				_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription				_pps_capture_sub{ORB_ID(pps_capture)};
-
-	// Trigger Buffer
-	struct _trig_s {
-		uint32_t chan_index;
-		hrt_abstime hrt_edge_time;
-		uint32_t edge_state;
-		uint32_t overflow;
-	} _trigger{};
-
-	bool			_capture_enabled{false};
-	bool			_gpio_capture{false};
-
-	// Parameters
-	param_t 		_p_strobe_delay{PARAM_INVALID};
-	float			_strobe_delay{0.0f};
-	param_t			_p_camera_capture_mode{PARAM_INVALID};
-	int32_t			_camera_capture_mode{0};
-	param_t			_p_camera_capture_edge{PARAM_INVALID};
-	int32_t			_camera_capture_edge{0};
-
-	// Signal capture statistics
-	uint32_t		_capture_seq{0};
-	hrt_abstime		_last_trig_begin_time{0};
-	hrt_abstime		_last_exposure_time{0};
-	hrt_abstime		_last_trig_time{0};
-	uint32_t 		_capture_overflows{0};
-
-	hrt_abstime	_pps_hrt_timestamp{0};
-	uint64_t		_pps_rtc_timestamp{0};
-	uint8_t			_trigger_rate_exceeded_counter{0};
-	px4::atomic<bool> _trigger_rate_failure{false};
-
-	orb_advert_t _mavlink_log_pub{nullptr};
-
-	// Signal capture callback
-	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
-
-	// GPIO interrupt routine
-	static int		gpio_interrupt_routine(int irq, void *context, void *arg);
-
-	// Signal capture publish
-	static void		publish_trigger_trampoline(void *arg);
-
+    int _capture_channel = 5; ///< by default, use FMU output 6
+
+    // Publishers
+    uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+    uORB::Publication<camera_trigger_s>      _trigger_pub{ORB_ID(camera_trigger)};
+
+    // Subscribers
+    uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
+    uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
+
+    // Trigger Buffer
+    struct _trig_s {
+        uint32_t    chan_index;
+        hrt_abstime hrt_edge_time;
+        uint32_t    edge_state;
+        uint32_t    overflow;
+    } _trigger{};
+
+    bool _capture_enabled{false};
+    bool _gpio_capture{false};
+
+    // Parameters
+    param_t _p_strobe_delay{PARAM_INVALID};
+    float   _strobe_delay{0.0f};
+    param_t _p_camera_capture_mode{PARAM_INVALID};
+    int32_t _camera_capture_mode{0};
+    param_t _p_camera_capture_edge{PARAM_INVALID};
+    int32_t _camera_capture_edge{0};
+
+    // Signal capture statistics
+    uint32_t    _capture_seq{0};
+    hrt_abstime _last_trig_begin_time{0};
+    hrt_abstime _last_exposure_time{0};
+    hrt_abstime _last_trig_time{0};
+    uint32_t    _capture_overflows{0};
+
+    hrt_abstime       _pps_hrt_timestamp{0};
+    uint64_t          _pps_rtc_timestamp{0};
+    uint8_t           _trigger_rate_exceeded_counter{0};
+    px4::atomic<bool> _trigger_rate_failure{false};
+
+    orb_advert_t _mavlink_log_pub{nullptr};
+
+    // Signal capture callback
+    void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
+
+    // GPIO interrupt routine
+    static int gpio_interrupt_routine(int irq, void *context, void *arg);
+
+    // Signal capture publish
+    static void publish_trigger_trampoline(void *arg);
 };
diff --git a/apps/peripheral/payload/camera_capture/camera_capture_params.c b/apps/peripheral/payload/camera_capture/camera_capture_params.c
index f82d0318bf..57ab57da07 100644
--- a/apps/peripheral/payload/camera_capture/camera_capture_params.c
+++ b/apps/peripheral/payload/camera_capture/camera_capture_params.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_capture_params.c
@@ -84,4 +61,4 @@ PARAM_DEFINE_INT32(CAM_CAP_MODE, 0);
  * @group Camera Control
  * @reboot_required true
  */
-PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
\ No newline at end of file
+PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
diff --git a/apps/peripheral/payload/camera_trigger/camera_trigger.cpp b/apps/peripheral/payload/camera_trigger/camera_trigger.cpp
index c03e63aca4..132981aab9 100644
--- a/apps/peripheral/payload/camera_trigger/camera_trigger.cpp
+++ b/apps/peripheral/payload/camera_trigger/camera_trigger.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_trigger.cpp
@@ -72,909 +49,848 @@
 #include "interfaces/src/seagull_map2.h"
 
 typedef enum : int32_t {
-	CAMERA_INTERFACE_MODE_NONE = 0,
-	CAMERA_INTERFACE_MODE_GPIO,
-	CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM,
-	CAMERA_INTERFACE_MODE_MAVLINK,
-	CAMERA_INTERFACE_MODE_GENERIC_PWM
+    CAMERA_INTERFACE_MODE_NONE = 0,
+    CAMERA_INTERFACE_MODE_GPIO,
+    CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM,
+    CAMERA_INTERFACE_MODE_MAVLINK,
+    CAMERA_INTERFACE_MODE_GENERIC_PWM
 } camera_interface_mode_t;
 
 typedef enum : int32_t {
-	TRIGGER_MODE_NONE = 0,
-	TRIGGER_MODE_INTERVAL_ON_CMD,
-	TRIGGER_MODE_INTERVAL_ALWAYS_ON,
-	TRIGGER_MODE_DISTANCE_ALWAYS_ON,
-	TRIGGER_MODE_DISTANCE_ON_CMD
+    TRIGGER_MODE_NONE = 0,
+    TRIGGER_MODE_INTERVAL_ON_CMD,
+    TRIGGER_MODE_INTERVAL_ALWAYS_ON,
+    TRIGGER_MODE_DISTANCE_ALWAYS_ON,
+    TRIGGER_MODE_DISTANCE_ON_CMD
 } trigger_mode_t;
 
 #define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
 
-class CameraTrigger : public px4::ScheduledWorkItem
-{
+class CameraTrigger : public px4::ScheduledWorkItem {
 public:
-	/**
+    /**
 	 * Constructor
 	 */
-	CameraTrigger();
+    CameraTrigger();
 
-	/**
+    /**
 	 * Destructor, also kills task.
 	 */
-	~CameraTrigger() override;
+    ~CameraTrigger() override;
 
-	/**
+    /**
 	 * Run intervalometer update
 	 */
-	void		update_intervalometer();
+    void update_intervalometer();
 
-	/**
+    /**
 	 * Run distance-based trigger update
 	 */
-	void		update_distance();
+    void update_distance();
 
-	/**
+    /**
 	 * Trigger the camera just once
 	 */
-	void		shoot_once();
+    void shoot_once();
 
-	/**
+    /**
 	 * Toggle keep camera alive functionality
 	 */
-	void		enable_keep_alive(bool on);
+    void enable_keep_alive(bool on);
 
-	/**
+    /**
 	 * Toggle camera power (on/off)
 	 */
-	void        	toggle_power();
+    void toggle_power();
 
-	/**
+    /**
 	 * Start the task.
 	 */
-	bool		start();
+    bool start();
 
-	/**
+    /**
 	 * Stop the task.
 	 */
-	void		stop();
+    void stop();
 
-	/**
+    /**
 	 * Display status.
 	 */
-	void		status();
+    void status();
 
-	/**
+    /**
 	 * Trigger one image
 	 */
-	void		test();
+    void test();
 
-	/**
+    /**
 	 * adjusts pose between triggers in CAMPOS mode
 	 */
-	void		adjust_roll();
+    void adjust_roll();
 
 private:
+    struct hrt_call _engagecall {};
+
+    struct hrt_call _disengagecall {};
+
+    struct hrt_call _engage_turn_on_off_call {};
+
+    struct hrt_call _disengage_turn_on_off_call {};
+
+    struct hrt_call _keepalivecall_up {};
+
+    struct hrt_call _keepalivecall_down {};
+
+    float            _activation_time{0.5f};
+    float            _interval{100.f};
+    float            _min_interval{1.f};
+    float            _distance{25.f};
+    uint32_t         _trigger_seq{0};
+    hrt_abstime      _last_trigger_timestamp{0};
+    bool             _trigger_enabled{false};
+    bool             _trigger_paused{false};
+    bool             _one_shot{false};
+    bool             _test_shot{false};
+    bool             _turning_on{false};
+    matrix::Vector2f _last_shoot_position{0.f, 0.f};
+    bool             _valid_position{false};
+    hrt_abstime      _pps_hrt_timestamp{0};
+    uint64_t         _pps_rtc_timestamp{0};
+
+    //Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
+    uint32_t _CAMPOS_num_poses{0};
+    uint32_t _CAMPOS_pose_counter{0};
+    float    _CAMPOS_roll_angle{0.f};
+    float    _CAMPOS_angle_interval{0.f};
+    float    _CAMPOS_pitch_angle{-90.f};
+    bool     _CAMPOS_updated_roll_angle{false};
+    uint32_t _target_system{0};
+    uint32_t _target_component{0};
+
+    uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
+    uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
+    uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)};
 
-	struct hrt_call _engagecall {};
-	struct hrt_call _disengagecall {};
-	struct hrt_call _engage_turn_on_off_call {};
-	struct hrt_call _disengage_turn_on_off_call {};
-	struct hrt_call _keepalivecall_up {};
-	struct hrt_call _keepalivecall_down {};
-
-	float			_activation_time{0.5f};
-	float			_interval{100.f};
-	float			_min_interval{1.f};
-	float 			_distance{25.f};
-	uint32_t 		_trigger_seq{0};
-	hrt_abstime		_last_trigger_timestamp{0};
-	bool			_trigger_enabled{false};
-	bool			_trigger_paused{false};
-	bool			_one_shot{false};
-	bool			_test_shot{false};
-	bool 			_turning_on{false};
-	matrix::Vector2f	_last_shoot_position{0.f, 0.f};
-	bool			_valid_position{false};
-	hrt_abstime	_pps_hrt_timestamp{0};
-	uint64_t		_pps_rtc_timestamp{0};
-
-	//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
-	uint32_t		_CAMPOS_num_poses{0};
-	uint32_t		_CAMPOS_pose_counter{0};
-	float			_CAMPOS_roll_angle{0.f};
-	float			_CAMPOS_angle_interval{0.f};
-	float			_CAMPOS_pitch_angle{-90.f};
-	bool			_CAMPOS_updated_roll_angle{false};
-	uint32_t		_target_system{0};
-	uint32_t		_target_component{0};
-
-	uORB::Subscription	_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_lpos_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription	_pps_capture_sub{ORB_ID(pps_capture)};
-
-	orb_advert_t		_trigger_pub{nullptr};
-
-	uORB::Publication<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
-
-	param_t			_p_mode;
-	param_t			_p_activation_time;
-	param_t			_p_interval;
-	param_t			_p_min_interval;
-	param_t			_p_distance;
-	param_t			_p_interface;
-
-	trigger_mode_t		_trigger_mode{TRIGGER_MODE_NONE};
-
-	camera_interface_mode_t	_camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
-	CameraInterface		*_camera_interface{nullptr};  ///< instance of camera interface
-
-	/**
+    orb_advert_t _trigger_pub{nullptr};
+
+    uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+
+    param_t _p_mode;
+    param_t _p_activation_time;
+    param_t _p_interval;
+    param_t _p_min_interval;
+    param_t _p_distance;
+    param_t _p_interface;
+
+    trigger_mode_t _trigger_mode{TRIGGER_MODE_NONE};
+
+    camera_interface_mode_t _camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
+    CameraInterface        *_camera_interface{nullptr}; ///< instance of camera interface
+
+    /**
 	 * Vehicle command handler
 	 */
-	void		Run() override;
+    void Run() override;
 
-	/**
+    /**
 	 * Fires trigger
 	 */
-	static void	engage(void *arg);
+    static void engage(void *arg);
 
-	/**
+    /**
 	 * Resets trigger
 	 */
-	static void	disengage(void *arg);
+    static void disengage(void *arg);
 
-	/**
+    /**
 	 * Fires on/off
 	 */
-	static void engange_turn_on_off(void *arg);
+    static void engange_turn_on_off(void *arg);
 
-	/**
+    /**
 	 * Resets  on/off
 	 */
-	static void disengage_turn_on_off(void *arg);
+    static void disengage_turn_on_off(void *arg);
 
-	/**
+    /**
 	 * Enables keep alive signal
 	 */
-	static void	keep_alive_up(void *arg);
-	/**
+    static void keep_alive_up(void *arg);
+    /**
 	 * Disables keep alive signal
 	 */
-	static void	keep_alive_down(void *arg);
-
+    static void keep_alive_down(void *arg);
 };
 
-namespace camera_trigger
-{
-CameraTrigger	*g_camera_trigger;
+namespace camera_trigger {
+CameraTrigger *g_camera_trigger;
 }
 
 CameraTrigger::CameraTrigger() :
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-	// Initiate camera interface based on camera_interface_mode
-	if (_camera_interface != nullptr) {
-		delete (_camera_interface);
-		// set to zero to ensure parser is not used while not instantiated
-		_camera_interface = nullptr;
-	}
-
-	// Parameters
-	_p_interval = param_find("TRIG_INTERVAL");
-	_p_min_interval = param_find("TRIG_MIN_INTERVA");
-	_p_distance = param_find("TRIG_DISTANCE");
-	_p_activation_time = param_find("TRIG_ACT_TIME");
-	_p_mode = param_find("TRIG_MODE");
-	_p_interface = param_find("TRIG_INTERFACE");
-
-	param_get(_p_activation_time, &_activation_time);
-	param_get(_p_interval, &_interval);
-	param_get(_p_min_interval, &_min_interval);
-	param_get(_p_distance, &_distance);
-	param_get(_p_mode, (int32_t *)&_trigger_mode);
-	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
-
-	switch (_camera_interface_mode) {
+    ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) {
+    // Initiate camera interface based on camera_interface_mode
+    if (_camera_interface != nullptr) {
+        delete (_camera_interface);
+        // set to zero to ensure parser is not used while not instantiated
+        _camera_interface = nullptr;
+    }
+
+    // Parameters
+    _p_interval        = param_find("TRIG_INTERVAL");
+    _p_min_interval    = param_find("TRIG_MIN_INTERVA");
+    _p_distance        = param_find("TRIG_DISTANCE");
+    _p_activation_time = param_find("TRIG_ACT_TIME");
+    _p_mode            = param_find("TRIG_MODE");
+    _p_interface       = param_find("TRIG_INTERFACE");
+
+    param_get(_p_activation_time, &_activation_time);
+    param_get(_p_interval, &_interval);
+    param_get(_p_min_interval, &_min_interval);
+    param_get(_p_distance, &_distance);
+    param_get(_p_mode, (int32_t *)&_trigger_mode);
+    param_get(_p_interface, (int32_t *)&_camera_interface_mode);
+
+    switch (_camera_interface_mode) {
 #ifdef __PX4_NUTTX
 
-	case CAMERA_INTERFACE_MODE_GPIO:
-		_camera_interface = new CameraInterfaceGPIO();
-		break;
+    case CAMERA_INTERFACE_MODE_GPIO:
+        _camera_interface = new CameraInterfaceGPIO();
+        break;
 
-	case CAMERA_INTERFACE_MODE_GENERIC_PWM:
-		_camera_interface = new CameraInterfacePWM();
-		break;
+    case CAMERA_INTERFACE_MODE_GENERIC_PWM:
+        _camera_interface = new CameraInterfacePWM();
+        break;
 
-	case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
-		_camera_interface = new CameraInterfaceSeagull();
-		break;
+    case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
+        _camera_interface = new CameraInterfaceSeagull();
+        break;
 
 #endif
 
-	case CAMERA_INTERFACE_MODE_MAVLINK:
-		// start an interface that does nothing. Instead mavlink will listen to the camera_trigger uORB message
-		_camera_interface = new CameraInterface();
-		break;
+    case CAMERA_INTERFACE_MODE_MAVLINK:
+        // start an interface that does nothing. Instead mavlink will listen to the camera_trigger uORB message
+        _camera_interface = new CameraInterface();
+        break;
 
-	default:
-		PX4_ERR("unknown camera interface mode: %d", static_cast<int>(_camera_interface_mode));
-		break;
-	}
+    default:
+        PX4_ERR("unknown camera interface mode: %d", static_cast<int>(_camera_interface_mode));
+        break;
+    }
 
-	// Enforce a lower bound on the activation interval in PWM modes to not miss
-	// engage calls in-between 50Hz PWM pulses. (see PX4 PR #6973)
-	if ((_activation_time < 40.0f) &&
-	    (_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM ||
-	     _camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
+    // Enforce a lower bound on the activation interval in PWM modes to not miss
+    // engage calls in-between 50Hz PWM pulses. (see PX4 PR #6973)
+    if ((_activation_time < 40.0f) && (_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM || _camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
+        _activation_time = 40.0f;
+        PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
+        param_set_no_notification(_p_activation_time, &(_activation_time));
+    }
 
-		_activation_time = 40.0f;
-		PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
-		param_set_no_notification(_p_activation_time, &(_activation_time));
-	}
+    // Advertise critical publishers here, because we cannot advertise in interrupt context
+    camera_trigger_s trigger{};
 
-	// Advertise critical publishers here, because we cannot advertise in interrupt context
-	camera_trigger_s trigger{};
-
-	_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
+    _trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
 }
 
-CameraTrigger::~CameraTrigger()
-{
-	if (_camera_interface != nullptr) {
-		delete (_camera_interface);
-	}
+CameraTrigger::~CameraTrigger() {
+    if (_camera_interface != nullptr) {
+        delete (_camera_interface);
+    }
 
-	camera_trigger::g_camera_trigger = nullptr;
+    camera_trigger::g_camera_trigger = nullptr;
 }
 
-void
-CameraTrigger::update_intervalometer()
-{
-	// the actual intervalometer runs in interrupt context, so we only need to call
-	// control_intervalometer once on enabling/disabling trigger to schedule the calls.
-
-	if (_trigger_enabled && !_trigger_paused) {
-		PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
+void CameraTrigger::update_intervalometer() {
+    // the actual intervalometer runs in interrupt context, so we only need to call
+    // control_intervalometer once on enabling/disabling trigger to schedule the calls.
 
-		// schedule trigger on and off calls
-		hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this);
+    if (_trigger_enabled && !_trigger_paused) {
+        PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
 
-		// schedule trigger on and off calls
-		hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), &CameraTrigger::disengage, this);
+        // schedule trigger on and off calls
+        hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this);
 
-	}
+        // schedule trigger on and off calls
+        hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), &CameraTrigger::disengage, this);
+    }
 }
 
-void
-CameraTrigger::update_distance()
-{
-	if (_turning_on || !_trigger_enabled || _trigger_paused) {
-		return;
-	}
-
-	vehicle_local_position_s local{};
-	_lpos_sub.copy(&local);
-
-	if (local.xy_valid) {
-		// Initialize position if not done yet
-		matrix::Vector2f current_position(local.x, local.y);
-
-		if (!_valid_position) {
-			// First time valid position, take first shot
-			_last_shoot_position = current_position;
-			_valid_position = local.xy_valid;
-
-			if (!_one_shot) {
-				shoot_once();
-			}
-		}
-
-		hrt_abstime now = hrt_absolute_time();
-
-		if (!_CAMPOS_updated_roll_angle && _CAMPOS_num_poses > 0 && (now - _last_trigger_timestamp > _min_interval * 1000)) {
-			adjust_roll();
-			_CAMPOS_updated_roll_angle = true;
-		}
-
-		// Check that distance threshold is exceeded
-		if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
-			shoot_once();
-			_CAMPOS_updated_roll_angle = false;
-			_last_shoot_position = current_position;
-		}
-	}
+void CameraTrigger::update_distance() {
+    if (_turning_on || !_trigger_enabled || _trigger_paused) {
+        return;
+    }
+
+    vehicle_local_position_s local{};
+    _lpos_sub.copy(&local);
+
+    if (local.xy_valid) {
+        // Initialize position if not done yet
+        matrix::Vector2f current_position(local.x, local.y);
+
+        if (!_valid_position) {
+            // First time valid position, take first shot
+            _last_shoot_position = current_position;
+            _valid_position      = local.xy_valid;
+
+            if (!_one_shot) {
+                shoot_once();
+            }
+        }
+
+        hrt_abstime now = hrt_absolute_time();
+
+        if (!_CAMPOS_updated_roll_angle && _CAMPOS_num_poses > 0 && (now - _last_trigger_timestamp > _min_interval * 1000)) {
+            adjust_roll();
+            _CAMPOS_updated_roll_angle = true;
+        }
+
+        // Check that distance threshold is exceeded
+        if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
+            shoot_once();
+            _CAMPOS_updated_roll_angle = false;
+            _last_shoot_position       = current_position;
+        }
+    }
 }
 
-void
-CameraTrigger::enable_keep_alive(bool on)
-{
-	if (on) {
-		PX4_DEBUG("keep alive enable");
+void CameraTrigger::enable_keep_alive(bool on) {
+    if (on) {
+        PX4_DEBUG("keep alive enable");
 
-		// schedule keep-alive up and down calls
-		hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), &CameraTrigger::keep_alive_up, this);
+        // schedule keep-alive up and down calls
+        hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), &CameraTrigger::keep_alive_up, this);
 
-		hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), &CameraTrigger::keep_alive_down, this);
+        hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), &CameraTrigger::keep_alive_down, this);
 
-	} else {
-		PX4_DEBUG("keep alive disable");
-		// cancel all calls
-		hrt_cancel(&_keepalivecall_up);
-		hrt_cancel(&_keepalivecall_down);
-	}
+    } else {
+        PX4_DEBUG("keep alive disable");
+        // cancel all calls
+        hrt_cancel(&_keepalivecall_up);
+        hrt_cancel(&_keepalivecall_down);
+    }
 }
 
-void
-CameraTrigger::toggle_power()
-{
-	PX4_DEBUG("toggle power");
+void CameraTrigger::toggle_power() {
+    PX4_DEBUG("toggle power");
 
-	// schedule power toggle calls
-	hrt_call_after(&_engage_turn_on_off_call, 0, &CameraTrigger::engange_turn_on_off, this);
+    // schedule power toggle calls
+    hrt_call_after(&_engage_turn_on_off_call, 0, &CameraTrigger::engange_turn_on_off, this);
 
-	hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000), &CameraTrigger::disengage_turn_on_off, this);
+    hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000), &CameraTrigger::disengage_turn_on_off, this);
 }
 
-void
-CameraTrigger::shoot_once()
-{
-	PX4_DEBUG("shoot once");
+void CameraTrigger::shoot_once() {
+    PX4_DEBUG("shoot once");
 
-	// schedule trigger on and off calls
-	hrt_call_after(&_engagecall, 0,
-		       (hrt_callout)&CameraTrigger::engage, this);
+    // schedule trigger on and off calls
+    hrt_call_after(&_engagecall, 0,
+                   (hrt_callout)&CameraTrigger::engage, this);
 
-	hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), &CameraTrigger::disengage, this);
+    hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), &CameraTrigger::disengage, this);
 }
 
-bool
-CameraTrigger::start()
-{
-	if (_camera_interface == nullptr) {
-		if (camera_trigger::g_camera_trigger != nullptr) {
-			delete (camera_trigger::g_camera_trigger);
-			camera_trigger::g_camera_trigger = nullptr;
+bool CameraTrigger::start() {
+    if (_camera_interface == nullptr) {
+        if (camera_trigger::g_camera_trigger != nullptr) {
+            delete (camera_trigger::g_camera_trigger);
+            camera_trigger::g_camera_trigger = nullptr;
+        }
+
+        return false;
+    }
 
-		}
+    if ((_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON || _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) && _camera_interface->has_power_control() && !_camera_interface->is_powered_on()) {
+        // If in always-on mode and the interface supports it, enable power to the camera
+        toggle_power();
+        enable_keep_alive(true);
 
-		return false;
-	}
+    } else {
+        enable_keep_alive(false);
+    }
 
-	if ((_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON ||
-	     _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) &&
-	    _camera_interface->has_power_control() &&
-	    !_camera_interface->is_powered_on()) {
+    // enable immediately if configured that way
+    if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON) {
+        // enable and start triggering
+        _trigger_enabled = true;
+        update_intervalometer();
 
-		// If in always-on mode and the interface supports it, enable power to the camera
-		toggle_power();
-		enable_keep_alive(true);
+    } else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
+        // just enable, but do not fire. actual trigger is based on distance covered
+        _trigger_enabled = true;
+    }
 
-	} else {
-		enable_keep_alive(false);
-	}
+    // start to monitor at high rate for trigger enable command
+    ScheduleNow();
 
-	// enable immediately if configured that way
-	if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON) {
-		// enable and start triggering
-		_trigger_enabled = true;
-		update_intervalometer();
+    return true;
+}
 
-	} else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
-		// just enable, but do not fire. actual trigger is based on distance covered
-		_trigger_enabled = true;
-	}
+void CameraTrigger::stop() {
+    ScheduleClear();
 
-	// start to monitor at high rate for trigger enable command
-	ScheduleNow();
+    hrt_cancel(&_engagecall);
+    hrt_cancel(&_disengagecall);
+    hrt_cancel(&_engage_turn_on_off_call);
+    hrt_cancel(&_disengage_turn_on_off_call);
+    hrt_cancel(&_keepalivecall_up);
+    hrt_cancel(&_keepalivecall_down);
 
-	return true;
+    if (camera_trigger::g_camera_trigger != nullptr) {
+        delete (camera_trigger::g_camera_trigger);
+        camera_trigger::g_camera_trigger = nullptr;
+    }
 }
 
-void
-CameraTrigger::stop()
-{
-	ScheduleClear();
-
-	hrt_cancel(&_engagecall);
-	hrt_cancel(&_disengagecall);
-	hrt_cancel(&_engage_turn_on_off_call);
-	hrt_cancel(&_disengage_turn_on_off_call);
-	hrt_cancel(&_keepalivecall_up);
-	hrt_cancel(&_keepalivecall_down);
-
-	if (camera_trigger::g_camera_trigger != nullptr) {
-		delete (camera_trigger::g_camera_trigger);
-		camera_trigger::g_camera_trigger = nullptr;
-	}
-}
+void CameraTrigger::test() {
+    vehicle_command_s vcmd{};
+    vcmd.param5           = 1.0;
+    vcmd.command          = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
+    vcmd.target_system    = 1;
+    vcmd.target_component = 1;
 
-void
-CameraTrigger::test()
-{
-	vehicle_command_s vcmd{};
-	vcmd.param5 = 1.0;
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
-	vcmd.target_system = 1;
-	vcmd.target_component = 1;
-
-	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
-	vcmd.timestamp = hrt_absolute_time();
-	vcmd_pub.publish(vcmd);
+    uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+    vcmd.timestamp = hrt_absolute_time();
+    vcmd_pub.publish(vcmd);
 }
 
-void
-CameraTrigger::Run()
-{
-	// default loop polling interval
-	int poll_interval_usec = 50000;
+void CameraTrigger::Run() {
+    // default loop polling interval
+    int poll_interval_usec = 50000;
 
-	vehicle_command_s cmd{};
-	unsigned cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-	bool need_ack = false;
+    vehicle_command_s cmd{};
+    unsigned          cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+    bool              need_ack   = false;
 
-	// this flag is set when the polling loop is slowed down to allow the camera to power on
-	_turning_on = false;
+    // this flag is set when the polling loop is slowed down to allow the camera to power on
+    _turning_on = false;
 
-	// these flags are used to detect state changes in the command loop
-	bool previous_trigger_state = _trigger_enabled;
-	bool previous_trigger_paused = _trigger_paused;
+    // these flags are used to detect state changes in the command loop
+    bool previous_trigger_state  = _trigger_enabled;
+    bool previous_trigger_paused = _trigger_paused;
 
-	bool updated = _command_sub.update(&cmd);
+    bool updated = _command_sub.update(&cmd);
 
-	// Command handling
-	if (updated) {
-		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
-			PX4_DEBUG("received DO_DIGICAM_CONTROL");
+    // Command handling
+    if (updated) {
+        if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
+            PX4_DEBUG("received DO_DIGICAM_CONTROL");
 
-			need_ack = true;
-			hrt_abstime now = hrt_absolute_time();
+            need_ack        = true;
+            hrt_abstime now = hrt_absolute_time();
 
-			if (now - _last_trigger_timestamp < _min_interval * 1000) {
-				// triggering too fast, abort
-				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+            if (now - _last_trigger_timestamp < _min_interval * 1000) {
+                // triggering too fast, abort
+                cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
-			} else {
-				if (commandParamToInt(cmd.param7) == 1) {
-					// test shots are not logged or forwarded to GCS for geotagging
-					_test_shot = true;
-				}
+            } else {
+                if (commandParamToInt(cmd.param7) == 1) {
+                    // test shots are not logged or forwarded to GCS for geotagging
+                    _test_shot = true;
+                }
 
-				if (commandParamToInt((float)cmd.param5) == 1) {
-					// Schedule shot
-					_one_shot = true;
-				}
+                if (commandParamToInt((float)cmd.param5) == 1) {
+                    // Schedule shot
+                    _one_shot = true;
+                }
 
-				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-			}
+                cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+            }
 
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
-			PX4_DEBUG("received DO_TRIGGER_CONTROL");
-			need_ack = true;
+        } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
+            PX4_DEBUG("received DO_TRIGGER_CONTROL");
+            need_ack = true;
 
-			if (commandParamToInt(cmd.param3) == 1) {
-				// pause triggger
-				_trigger_paused = true;
+            if (commandParamToInt(cmd.param3) == 1) {
+                // pause triggger
+                _trigger_paused = true;
 
-			} else if (commandParamToInt(cmd.param3) == 0) {
-				_trigger_paused = false;
-			}
+            } else if (commandParamToInt(cmd.param3) == 0) {
+                _trigger_paused = false;
+            }
 
-			if (commandParamToInt(cmd.param2) == 1) {
-				// reset trigger sequence
-				_trigger_seq = 0;
-			}
+            if (commandParamToInt(cmd.param2) == 1) {
+                // reset trigger sequence
+                _trigger_seq = 0;
+            }
 
-			if (commandParamToInt(cmd.param1) == 1) {
-				_trigger_enabled = true;
+            if (commandParamToInt(cmd.param1) == 1) {
+                _trigger_enabled = true;
 
-			} else if (commandParamToInt(cmd.param1) == 0) {
-				_trigger_enabled = false;
-			}
+            } else if (commandParamToInt(cmd.param1) == 0) {
+                _trigger_enabled = false;
+            }
 
-			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+            cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
-			PX4_DEBUG("received DO_SET_CAM_TRIGG_DIST");
-			need_ack = true;
+        } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
+            PX4_DEBUG("received DO_SET_CAM_TRIGG_DIST");
+            need_ack = true;
 
-			/*
+            /*
 			 * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
 			 */
-			if (cmd.param1 > 0.0f) {
-				_distance = cmd.param1;
-				param_set_no_notification(_p_distance, &_distance);
-
-				_trigger_enabled = true;
-				_trigger_paused = false;
-				_valid_position = false;
-
-			} else if (commandParamToInt(cmd.param1) == 0) {
-				_trigger_paused = true;
-
-			} else if (commandParamToInt(cmd.param1) == -1) {
-				_trigger_enabled = false;
-			}
-
-			// We can only control the shutter integration time of the camera in GPIO mode (for now)
-			if (cmd.param2 > 0.0f) {
-				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
-					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &(_activation_time));
-				}
-			}
-
-			// Trigger once immediately if param is set
-			if (cmd.param3 > 0.0f) {
-				// Schedule shot
-				_one_shot = true;
-			}
-
-			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
-			PX4_DEBUG("received DO_SET_CAM_TRIGG_INTERVAL");
-			need_ack = true;
-
-			if (cmd.param1 > 0.0f) {
-				_interval = cmd.param1;
-				param_set_no_notification(_p_interval, &(_interval));
-			}
-
-			// We can only control the shutter integration time of the camera in GPIO mode
-			if (cmd.param2 > 0.0f) {
-				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
-					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &_activation_time);
-				}
-			}
-
-			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
-			PX4_INFO("received OBLIQUE_SURVEY");
-			// Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
-
-			need_ack = true;
-
-			if (cmd.param1 > 0.0f) {
-				_distance = cmd.param1;
-				param_set_no_notification(_p_distance, &_distance);
-
-				_trigger_enabled = true;
-				_trigger_paused = false;
-				_valid_position = false;
-
-			} else if (commandParamToInt(cmd.param1) == 0) {
-				_trigger_paused = true;
-
-			} else if (commandParamToInt(cmd.param1) == -1) {
-				_trigger_enabled = false;
-			}
-
-			// We can only control the shutter integration time of the camera in GPIO mode (for now)
-			if (cmd.param2 > 0.0f) {
-				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
-					_activation_time = cmd.param2;
-					param_set_no_notification(_p_activation_time, &(_activation_time));
-				}
-			}
-
-			// Set Param for minimum camera trigger interval
-			if (cmd.param3 > 0.0f) {
-				_min_interval = cmd.param3;
-				param_set_no_notification(_p_min_interval, &(_min_interval));
-			}
-
-			if (cmd.param4 >= 2.0f) {
-				_CAMPOS_num_poses = commandParamToInt(cmd.param4);
-				_CAMPOS_roll_angle = cmd.param5;
-				_CAMPOS_pitch_angle = cmd.param6;
-				_CAMPOS_angle_interval = _CAMPOS_roll_angle * 2 / (_CAMPOS_num_poses - 1);
-				_CAMPOS_pose_counter = 0;
-				_CAMPOS_updated_roll_angle = false;
-
-			} else {
-				_CAMPOS_num_poses = 0;
-			}
-
-			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else {
-			goto unknown_cmd;
-		}
-
-		_target_system = cmd.target_system;
-		_target_component = cmd.target_component;
-	}
+            if (cmd.param1 > 0.0f) {
+                _distance = cmd.param1;
+                param_set_no_notification(_p_distance, &_distance);
+
+                _trigger_enabled = true;
+                _trigger_paused  = false;
+                _valid_position  = false;
+
+            } else if (commandParamToInt(cmd.param1) == 0) {
+                _trigger_paused = true;
+
+            } else if (commandParamToInt(cmd.param1) == -1) {
+                _trigger_enabled = false;
+            }
+
+            // We can only control the shutter integration time of the camera in GPIO mode (for now)
+            if (cmd.param2 > 0.0f) {
+                if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+                    _activation_time = cmd.param2;
+                    param_set_no_notification(_p_activation_time, &(_activation_time));
+                }
+            }
+
+            // Trigger once immediately if param is set
+            if (cmd.param3 > 0.0f) {
+                // Schedule shot
+                _one_shot = true;
+            }
+
+            cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+        } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
+            PX4_DEBUG("received DO_SET_CAM_TRIGG_INTERVAL");
+            need_ack = true;
+
+            if (cmd.param1 > 0.0f) {
+                _interval = cmd.param1;
+                param_set_no_notification(_p_interval, &(_interval));
+            }
+
+            // We can only control the shutter integration time of the camera in GPIO mode
+            if (cmd.param2 > 0.0f) {
+                if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+                    _activation_time = cmd.param2;
+                    param_set_no_notification(_p_activation_time, &_activation_time);
+                }
+            }
+
+            cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+        } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
+            PX4_INFO("received OBLIQUE_SURVEY");
+            // Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
+
+            need_ack = true;
+
+            if (cmd.param1 > 0.0f) {
+                _distance = cmd.param1;
+                param_set_no_notification(_p_distance, &_distance);
+
+                _trigger_enabled = true;
+                _trigger_paused  = false;
+                _valid_position  = false;
+
+            } else if (commandParamToInt(cmd.param1) == 0) {
+                _trigger_paused = true;
+
+            } else if (commandParamToInt(cmd.param1) == -1) {
+                _trigger_enabled = false;
+            }
+
+            // We can only control the shutter integration time of the camera in GPIO mode (for now)
+            if (cmd.param2 > 0.0f) {
+                if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+                    _activation_time = cmd.param2;
+                    param_set_no_notification(_p_activation_time, &(_activation_time));
+                }
+            }
+
+            // Set Param for minimum camera trigger interval
+            if (cmd.param3 > 0.0f) {
+                _min_interval = cmd.param3;
+                param_set_no_notification(_p_min_interval, &(_min_interval));
+            }
+
+            if (cmd.param4 >= 2.0f) {
+                _CAMPOS_num_poses          = commandParamToInt(cmd.param4);
+                _CAMPOS_roll_angle         = cmd.param5;
+                _CAMPOS_pitch_angle        = cmd.param6;
+                _CAMPOS_angle_interval     = _CAMPOS_roll_angle * 2 / (_CAMPOS_num_poses - 1);
+                _CAMPOS_pose_counter       = 0;
+                _CAMPOS_updated_roll_angle = false;
+
+            } else {
+                _CAMPOS_num_poses = 0;
+            }
+
+            cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+        } else {
+            goto unknown_cmd;
+        }
+
+        _target_system    = cmd.target_system;
+        _target_component = cmd.target_component;
+    }
 
 unknown_cmd:
 
-	// State change handling
-	if ((previous_trigger_state != _trigger_enabled) ||
-	    (previous_trigger_paused != _trigger_paused) ||
-	    _one_shot) {
-
-		if (_trigger_enabled || _one_shot) { // Just got enabled via a command
-
-			// If camera isn't already powered on, we enable power to it
-			if (!_camera_interface->is_powered_on() &&
-			    _camera_interface->has_power_control()) {
-
-				toggle_power();
-				enable_keep_alive(true);
-
-				// Give the camera time to turn on before starting to send trigger signals
-				poll_interval_usec = 3000000;
-				_turning_on = true;
-			}
-		}
-
-		if ((!_trigger_enabled || _trigger_paused) && !_one_shot) { // Just got disabled/paused via a command
-
-			// Power off the camera if we are disabled
-			if (_camera_interface->is_powered_on() &&
-			    _camera_interface->has_power_control() &&
-			    !_trigger_enabled) {
-
-				enable_keep_alive(false);
-				toggle_power();
-			}
-
-			// cancel all calls for both disabled and paused
-			hrt_cancel(&_engagecall);
-			hrt_cancel(&_disengagecall);
-
-			// ensure that the pin is off
-			hrt_call_after(&_disengagecall, 0, (hrt_callout)&CameraTrigger::disengage, this);
-
-			// reset distance counter if needed
-			if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
-			    _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
-
-				// this will force distance counter reinit on getting enabled/unpaused
-				_valid_position = false;
-			}
-		}
-
-		// only run on state changes, not every loop iteration
-		if (_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
-			// update intervalometer state and reset calls
-			update_intervalometer();
-		}
-	}
-
-	// order matters - one_shot has to be handled LAST
-	// as the other trigger handlers will back off from it
-
-	// run every loop iteration and trigger if needed
-	if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
-	    _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
-
-		// update distance counter and trigger
-		update_distance();
-	}
-
-	// One shot command-based capture handling
-	if (_one_shot && !_turning_on) {
-		// One-shot trigger
-		shoot_once();
-		_one_shot = false;
-
-		if (_test_shot) {
-			_test_shot = false;
-		}
-	}
-
-	// Command ACK handling
-	if (updated && need_ack) {
-		PX4_DEBUG("acknowledging command %" PRId32 ", result=%u", cmd.command, cmd_result);
-		vehicle_command_ack_s command_ack{};
-		command_ack.command = cmd.command;
-		command_ack.result = (uint8_t)cmd_result;
-		command_ack.target_system = cmd.source_system;
-		command_ack.target_component = cmd.source_component;
-		command_ack.timestamp = hrt_absolute_time();
-		_cmd_ack_pub.publish(command_ack);
-	}
-
-	ScheduleDelayed(poll_interval_usec);
+    // State change handling
+    if ((previous_trigger_state != _trigger_enabled) || (previous_trigger_paused != _trigger_paused) || _one_shot) {
+        if (_trigger_enabled || _one_shot) { // Just got enabled via a command
+
+            // If camera isn't already powered on, we enable power to it
+            if (!_camera_interface->is_powered_on() && _camera_interface->has_power_control()) {
+                toggle_power();
+                enable_keep_alive(true);
+
+                // Give the camera time to turn on before starting to send trigger signals
+                poll_interval_usec = 3000000;
+                _turning_on        = true;
+            }
+        }
+
+        if ((!_trigger_enabled || _trigger_paused) && !_one_shot) { // Just got disabled/paused via a command
+
+            // Power off the camera if we are disabled
+            if (_camera_interface->is_powered_on() && _camera_interface->has_power_control() && !_trigger_enabled) {
+                enable_keep_alive(false);
+                toggle_power();
+            }
+
+            // cancel all calls for both disabled and paused
+            hrt_cancel(&_engagecall);
+            hrt_cancel(&_disengagecall);
+
+            // ensure that the pin is off
+            hrt_call_after(&_disengagecall, 0, (hrt_callout)&CameraTrigger::disengage, this);
+
+            // reset distance counter if needed
+            if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD || _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
+                // this will force distance counter reinit on getting enabled/unpaused
+                _valid_position = false;
+            }
+        }
+
+        // only run on state changes, not every loop iteration
+        if (_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
+            // update intervalometer state and reset calls
+            update_intervalometer();
+        }
+    }
+
+    // order matters - one_shot has to be handled LAST
+    // as the other trigger handlers will back off from it
+
+    // run every loop iteration and trigger if needed
+    if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD || _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
+        // update distance counter and trigger
+        update_distance();
+    }
+
+    // One shot command-based capture handling
+    if (_one_shot && !_turning_on) {
+        // One-shot trigger
+        shoot_once();
+        _one_shot = false;
+
+        if (_test_shot) {
+            _test_shot = false;
+        }
+    }
+
+    // Command ACK handling
+    if (updated && need_ack) {
+        PX4_DEBUG("acknowledging command %" PRId32 ", result=%u", cmd.command, cmd_result);
+        vehicle_command_ack_s command_ack{};
+        command_ack.command          = cmd.command;
+        command_ack.result           = (uint8_t)cmd_result;
+        command_ack.target_system    = cmd.source_system;
+        command_ack.target_component = cmd.source_component;
+        command_ack.timestamp        = hrt_absolute_time();
+        _cmd_ack_pub.publish(command_ack);
+    }
+
+    ScheduleDelayed(poll_interval_usec);
 }
 
-void
-CameraTrigger::adjust_roll()
-{
-	vehicle_command_s vcmd{};
+void CameraTrigger::adjust_roll() {
+    vehicle_command_s vcmd{};
 
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
-	vcmd.target_system = _target_system;
-	vcmd.target_component = _target_component;
+    vcmd.command          = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
+    vcmd.target_system    = _target_system;
+    vcmd.target_component = _target_component;
 
-	//param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
-	vcmd.param1 = _CAMPOS_pitch_angle;
+    //param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
+    vcmd.param1 = _CAMPOS_pitch_angle;
 
-	//param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
-	if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
-		_CAMPOS_pose_counter = 0;
-	}
+    //param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
+    if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
+        _CAMPOS_pose_counter = 0;
+    }
 
-	vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
+    vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
 
-	vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
+    vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 
-	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
-	vcmd.timestamp = hrt_absolute_time();
-	vcmd_pub.publish(vcmd);
+    uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+    vcmd.timestamp = hrt_absolute_time();
+    vcmd_pub.publish(vcmd);
 }
 
-void
-CameraTrigger::engage(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::engage(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	hrt_abstime now = hrt_absolute_time();
+    hrt_abstime now = hrt_absolute_time();
 
-	if ((trig->_last_trigger_timestamp > 0) && (now - trig->_last_trigger_timestamp < trig->_min_interval * 1000)) {
-		return;
-	}
+    if ((trig->_last_trigger_timestamp > 0) && (now - trig->_last_trigger_timestamp < trig->_min_interval * 1000)) {
+        return;
+    }
 
-	// Trigger the camera
-	trig->_camera_interface->trigger(true);
-	// set last timestamp
-	trig->_last_trigger_timestamp = now;
+    // Trigger the camera
+    trig->_camera_interface->trigger(true);
+    // set last timestamp
+    trig->_last_trigger_timestamp = now;
 
-	if (trig->_test_shot) {
-		// do not send messages or increment frame count for test shots
-		return;
-	}
+    if (trig->_test_shot) {
+        // do not send messages or increment frame count for test shots
+        return;
+    }
 
-	pps_capture_s pps_capture;
+    pps_capture_s pps_capture;
 
-	if (trig->_pps_capture_sub.update(&pps_capture)) {
-		trig->_pps_hrt_timestamp = pps_capture.timestamp;
-		trig->_pps_rtc_timestamp = pps_capture.rtc_timestamp;
-	}
+    if (trig->_pps_capture_sub.update(&pps_capture)) {
+        trig->_pps_hrt_timestamp = pps_capture.timestamp;
+        trig->_pps_rtc_timestamp = pps_capture.rtc_timestamp;
+    }
 
-	// Send camera trigger message. This messages indicates that we sent
-	// the camera trigger request. Does not guarantee capture.
-	camera_trigger_s trigger{};
+    // Send camera trigger message. This messages indicates that we sent
+    // the camera trigger request. Does not guarantee capture.
+    camera_trigger_s trigger{};
 
-	if (trig->_pps_hrt_timestamp > 0) {
-		// Current RTC time (RTC time captured by the PPS module + elapsed time since capture)
-		trigger.timestamp_utc = trig->_pps_rtc_timestamp + hrt_elapsed_time(&trig->_pps_hrt_timestamp);
+    if (trig->_pps_hrt_timestamp > 0) {
+        // Current RTC time (RTC time captured by the PPS module + elapsed time since capture)
+        trigger.timestamp_utc = trig->_pps_rtc_timestamp + hrt_elapsed_time(&trig->_pps_hrt_timestamp);
 
-	} else {
-		// No PPS capture received, use RTC clock as fallback
-		timespec tv{};
-		px4_clock_gettime(CLOCK_REALTIME, &tv);
-		trigger.timestamp_utc = ts_to_abstime(&tv);
-	}
+    } else {
+        // No PPS capture received, use RTC clock as fallback
+        timespec tv{};
+        px4_clock_gettime(CLOCK_REALTIME, &tv);
+        trigger.timestamp_utc = ts_to_abstime(&tv);
+    }
 
-	trigger.seq = trig->_trigger_seq;
-	trigger.feedback = false;
-	trigger.timestamp = hrt_absolute_time();
+    trigger.seq       = trig->_trigger_seq;
+    trigger.feedback  = false;
+    trigger.timestamp = hrt_absolute_time();
 
-	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
+    orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
 
-	// increment frame count
-	trig->_trigger_seq++;
+    // increment frame count
+    trig->_trigger_seq++;
 }
 
-void
-CameraTrigger::disengage(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::disengage(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	trig->_camera_interface->trigger(false);
+    trig->_camera_interface->trigger(false);
 }
 
-void
-CameraTrigger::engange_turn_on_off(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::engange_turn_on_off(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	trig->_camera_interface->send_toggle_power(true);
+    trig->_camera_interface->send_toggle_power(true);
 }
 
-void
-CameraTrigger::disengage_turn_on_off(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::disengage_turn_on_off(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	trig->_camera_interface->send_toggle_power(false);
+    trig->_camera_interface->send_toggle_power(false);
 }
 
-void
-CameraTrigger::keep_alive_up(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::keep_alive_up(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	trig->_camera_interface->send_keep_alive(true);
+    trig->_camera_interface->send_keep_alive(true);
 }
 
-void
-CameraTrigger::keep_alive_down(void *arg)
-{
-	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
+void CameraTrigger::keep_alive_down(void *arg) {
+    CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
-	trig->_camera_interface->send_keep_alive(false);
+    trig->_camera_interface->send_keep_alive(false);
 }
 
-void
-CameraTrigger::status()
-{
-	PX4_INFO("trigger enabled : %s", _trigger_enabled ? "enabled" : "disabled");
-	PX4_INFO("trigger paused : %s", _trigger_paused ? "paused" : "active");
-	PX4_INFO("mode : %d", static_cast<int>(_trigger_mode));
+void CameraTrigger::status() {
+    PX4_INFO("trigger enabled : %s", _trigger_enabled ? "enabled" : "disabled");
+    PX4_INFO("trigger paused : %s", _trigger_paused ? "paused" : "active");
+    PX4_INFO("mode : %d", static_cast<int>(_trigger_mode));
 
-	if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON ||
-	    _trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
-		PX4_INFO("interval : %.2f [ms]", (double)_interval);
+    if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON || _trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
+        PX4_INFO("interval : %.2f [ms]", (double)_interval);
 
-	} else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON ||
-		   _trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD) {
-		PX4_INFO("distance : %.2f [m]", (double)_distance);
-	}
+    } else if (_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON || _trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD) {
+        PX4_INFO("distance : %.2f [m]", (double)_distance);
+    }
 
-	if (_camera_interface->has_power_control())	{
-		PX4_INFO("camera power : %s", _camera_interface->is_powered_on() ? "ON" : "OFF");
-	}
+    if (_camera_interface->has_power_control()) {
+        PX4_INFO("camera power : %s", _camera_interface->is_powered_on() ? "ON" : "OFF");
+    }
 
-	PX4_INFO("activation time : %.2f [ms]", (double)_activation_time);
-	_camera_interface->info();
+    PX4_INFO("activation time : %.2f [ms]", (double)_activation_time);
+    _camera_interface->info();
 }
 
-static int usage()
-{
-	PX4_INFO("usage: camera_trigger {start|stop|status|test|test_power}\n");
-	return 1;
+static int usage() {
+    PX4_INFO("usage: camera_trigger {start|stop|status|test|test_power}\n");
+    return 1;
 }
 
-extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[])
-{
-	if (argc < 2) {
-		return usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
+extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]) {
+    if (argc < 2) {
+        return usage();
+    }
 
-		if (camera_trigger::g_camera_trigger != nullptr) {
-			PX4_WARN("already running");
-			return 0;
-		}
+    if (!strcmp(argv[1], "start")) {
+        if (camera_trigger::g_camera_trigger != nullptr) {
+            PX4_WARN("already running");
+            return 0;
+        }
 
-		camera_trigger::g_camera_trigger = new CameraTrigger();
+        camera_trigger::g_camera_trigger = new CameraTrigger();
 
-		if (camera_trigger::g_camera_trigger == nullptr) {
-			PX4_WARN("alloc failed");
-			return 1;
-		}
+        if (camera_trigger::g_camera_trigger == nullptr) {
+            PX4_WARN("alloc failed");
+            return 1;
+        }
 
-		if (!camera_trigger::g_camera_trigger->start()) {
-			PX4_WARN("failed to start camera trigger");
-			return 1;
-		}
+        if (!camera_trigger::g_camera_trigger->start()) {
+            PX4_WARN("failed to start camera trigger");
+            return 1;
+        }
 
-		return 0;
-	}
+        return 0;
+    }
 
-	if (camera_trigger::g_camera_trigger == nullptr) {
-		PX4_WARN("not running");
-		return 1;
+    if (camera_trigger::g_camera_trigger == nullptr) {
+        PX4_WARN("not running");
+        return 1;
 
-	} else if (!strcmp(argv[1], "stop")) {
-		camera_trigger::g_camera_trigger->stop();
+    } else if (!strcmp(argv[1], "stop")) {
+        camera_trigger::g_camera_trigger->stop();
 
-	} else if (!strcmp(argv[1], "status")) {
-		camera_trigger::g_camera_trigger->status();
+    } else if (!strcmp(argv[1], "status")) {
+        camera_trigger::g_camera_trigger->status();
 
-	} else if (!strcmp(argv[1], "test")) {
-		camera_trigger::g_camera_trigger->test();
+    } else if (!strcmp(argv[1], "test")) {
+        camera_trigger::g_camera_trigger->test();
 
-	} else if (!strcmp(argv[1], "test_power")) {
-		camera_trigger::g_camera_trigger->toggle_power();
+    } else if (!strcmp(argv[1], "test_power")) {
+        camera_trigger::g_camera_trigger->toggle_power();
 
-	} else {
-		return usage();
-	}
+    } else {
+        return usage();
+    }
 
-	return 0;
+    return 0;
 }
diff --git a/apps/peripheral/payload/camera_trigger/camera_trigger_params.c b/apps/peripheral/payload/camera_trigger/camera_trigger_params.c
index 181c212e6a..8d4dd76347 100644
--- a/apps/peripheral/payload/camera_trigger/camera_trigger_params.c
+++ b/apps/peripheral/payload/camera_trigger/camera_trigger_params.c
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_trigger_params.c
@@ -163,4 +140,3 @@ PARAM_DEFINE_INT32(TRIG_PWM_SHOOT, 1900);
  * @reboot_required true
  */
 PARAM_DEFINE_INT32(TRIG_PWM_NEUTRAL, 1500);
-
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.cpp b/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.cpp
index 02e9119cae..b770064a25 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.cpp
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.cpp
@@ -1,59 +1,35 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "camera_interface.h"
 #include <px4_platform_common/log.h>
 #include <board_config.h>
 
-void CameraInterface::get_pins()
-{
-	// Set all pins as invalid
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		_pins[i] = -1;
-	}
+void CameraInterface::get_pins() {
+    // Set all pins as invalid
+    for (unsigned i = 0; i < arraySize(_pins); i++) {
+        _pins[i] = -1;
+    }
 
-	unsigned pin_index = 0;
+    unsigned pin_index = 0;
 
-	for (unsigned i = 0; i < 16 && pin_index < arraySize(_pins); ++i) {
-		char param_name[17];
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
-		param_t function_handle = param_find(param_name);
-		int32_t function;
+    for (unsigned i = 0; i < 16 && pin_index < arraySize(_pins); ++i) {
+        char param_name[17];
+        snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+        param_t function_handle = param_find(param_name);
+        int32_t function;
 
-		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
-			if (function == 2000) { // Camera_Trigger
-				_pins[pin_index++] = i;
-			}
-		}
-	}
+        if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+            if (function == 2000) { // Camera_Trigger
+                _pins[pin_index++] = i;
+            }
+        }
+    }
 }
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.h b/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.h
index f9079ea2ba..2e21c56a94 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.h
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/camera_interface.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file camera_interface.h
@@ -40,63 +17,69 @@
 #include <parameters/param.h>
 #include <px4_platform_common/log.h>
 
-#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
+#define arraySize(a) (sizeof((a)) / sizeof(((a)[0])))
 
-class CameraInterface
-{
+class CameraInterface {
 public:
-	CameraInterface() = default;
-	virtual ~CameraInterface() = default;
+    CameraInterface()          = default;
+    virtual ~CameraInterface() = default;
 
-	/**
+    /**
 	 * trigger the camera
 	 * @param enable
 	 */
-	virtual void trigger(bool trigger_on_true) {}
+    virtual void trigger(bool trigger_on_true) {
+    }
 
-	/**
+    /**
 	 * send command to turn the camera on/off
 	 * @param enable
 	 */
-	virtual void send_toggle_power(bool enable) {}
+    virtual void send_toggle_power(bool enable) {
+    }
 
-	/**
+    /**
 	 * send command to prevent the camera from sleeping
 	 * @param enable
 	 */
-	virtual void send_keep_alive(bool enable) {}
+    virtual void send_keep_alive(bool enable) {
+    }
 
-	/**
+    /**
 	 * Display info.
 	 */
-	virtual void info() {}
+    virtual void info() {
+    }
 
-	/**
+    /**
 	 * Checks if the interface has support for
 	 * camera power control
 	 * @return true if power control is supported
 	 */
-	virtual bool has_power_control() { return false; }
+    virtual bool has_power_control() {
+        return false;
+    }
 
-	/**
+    /**
 	 * Checks if the camera connected to the interface
 	 * is turned on.
 	 * @return true if camera is on
 	 */
-	virtual bool is_powered_on() { return true; }
+    virtual bool is_powered_on() {
+        return true;
+    }
 
 protected:
-
-	/**
+    /**
 	 * setup the interface
 	 */
-	virtual void setup() {}
+    virtual void setup() {
+    }
 
-	/**
+    /**
 	 * get the hardware configuration
 	 */
-	void get_pins();
-
-	int _pins[32] {};
+    void get_pins();
 
+    int _pins[32]{};
 };
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.cpp b/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.cpp
index 0aeb916020..9241f3bec0 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.cpp
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.cpp
@@ -1,113 +1,85 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifdef __PX4_NUTTX
 
-#include "gpio.h"
+#   include "gpio.h"
 
-#include <cstring>
-#include <px4_arch/io_timer.h>
+#   include <cstring>
+#   include <px4_arch/io_timer.h>
 
-CameraInterfaceGPIO::CameraInterfaceGPIO()
-{
-	_p_polarity = param_find("TRIG_POLARITY");
+CameraInterfaceGPIO::CameraInterfaceGPIO() {
+    _p_polarity = param_find("TRIG_POLARITY");
 
-	// polarity of the trigger (0 = active low, 1 = active high )
-	int32_t polarity = 0;
-	param_get(_p_polarity, &polarity);
-	_trigger_invert = (polarity == 0);
+    // polarity of the trigger (0 = active low, 1 = active high )
+    int32_t polarity = 0;
+    param_get(_p_polarity, &polarity);
+    _trigger_invert = (polarity == 0);
 
-	get_pins();
-	setup();
+    get_pins();
+    setup();
 }
 
-CameraInterfaceGPIO::~CameraInterfaceGPIO()
-{
-	unsigned channel = 0;
+CameraInterfaceGPIO::~CameraInterfaceGPIO() {
+    unsigned channel = 0;
 
-	while (_allocated_channels != 0) {
-		if (((1u << channel) & _allocated_channels)) {
-			io_timer_unallocate_channel(channel);
-			_allocated_channels &= ~(1u << channel);
-		}
+    while (_allocated_channels != 0) {
+        if (((1u << channel) & _allocated_channels)) {
+            io_timer_unallocate_channel(channel);
+            _allocated_channels &= ~(1u << channel);
+        }
 
-		++channel;
-	}
+        ++channel;
+    }
 }
 
-void CameraInterfaceGPIO::setup()
-{
-	_allocated_channels = 0;
-
-	for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
-		// Pin range is from 0 to num_gpios - 1
-		if (_pins[i] >= 0 && t < (int)arraySize(_triggers)) {
-			uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]);
-
-			if (io_timer_allocate_channel(_pins[i], IOTimerChanMode_Trigger) == 0) {
-				_allocated_channels |= 1 << _pins[i];
-				_triggers[t++] = gpio;
-				px4_arch_configgpio(gpio);
-				px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
-			}
-		}
-	}
+void CameraInterfaceGPIO::setup() {
+    _allocated_channels = 0;
+
+    for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
+        // Pin range is from 0 to num_gpios - 1
+        if (_pins[i] >= 0 && t < (int)arraySize(_triggers)) {
+            uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]);
+
+            if (io_timer_allocate_channel(_pins[i], IOTimerChanMode_Trigger) == 0) {
+                _allocated_channels |= 1 << _pins[i];
+                _triggers[t++]       = gpio;
+                px4_arch_configgpio(gpio);
+                px4_arch_gpiowrite(gpio, false ^ _trigger_invert);
+            }
+        }
+    }
 }
 
-void CameraInterfaceGPIO::trigger(bool trigger_on_true)
-{
-	bool trigger_state = trigger_on_true ^ _trigger_invert;
+void CameraInterfaceGPIO::trigger(bool trigger_on_true) {
+    bool trigger_state = trigger_on_true ^ _trigger_invert;
 
-	for (unsigned i = 0; i < arraySize(_triggers); i++) {
-		if (_triggers[i] != 0) {
-			px4_arch_gpiowrite(_triggers[i], trigger_state);
-		}
-	}
+    for (unsigned i = 0; i < arraySize(_triggers); i++) {
+        if (_triggers[i] != 0) {
+            px4_arch_gpiowrite(_triggers[i], trigger_state);
+        }
+    }
 }
 
-void CameraInterfaceGPIO::info()
-{
-	PX4_INFO_RAW("GPIO trigger mode, pins enabled: ");
+void CameraInterfaceGPIO::info() {
+    PX4_INFO_RAW("GPIO trigger mode, pins enabled: ");
 
-	for (unsigned i = 0; i < arraySize(_pins); ++i) {
-		if (_pins[i] < 0) {
-			continue;
-		}
+    for (unsigned i = 0; i < arraySize(_pins); ++i) {
+        if (_pins[i] < 0) {
+            continue;
+        }
 
-		PX4_INFO_RAW("[%d]", _pins[i] + 1);
-	}
+        PX4_INFO_RAW("[%d]", _pins[i] + 1);
+    }
 
-	PX4_INFO_RAW(", polarity : %s\n", _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH");
+    PX4_INFO_RAW(", polarity : %s\n", _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH");
 }
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.h b/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.h
index da81fa20e6..8d3e1cbcea 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.h
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/gpio.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file gpio.h
@@ -41,31 +18,30 @@
 
 #ifdef __PX4_NUTTX
 
-#include <board_config.h>
+#   include <board_config.h>
 
-#include "camera_interface.h"
+#   include "camera_interface.h"
 
-class CameraInterfaceGPIO : public CameraInterface
-{
+class CameraInterfaceGPIO : public CameraInterface {
 public:
-	CameraInterfaceGPIO();
-	virtual ~CameraInterfaceGPIO();
+    CameraInterfaceGPIO();
+    virtual ~CameraInterfaceGPIO();
 
-	void trigger(bool trigger_on_true);
+    void trigger(bool trigger_on_true);
 
-	void info();
+    void info();
 
 private:
-	static const int num_gpios = 32;
+    static const int num_gpios = 32;
 
-	void setup();
+    void setup();
 
-	param_t _p_polarity{PARAM_INVALID};
+    param_t _p_polarity{PARAM_INVALID};
 
-	bool _trigger_invert{false};
+    bool _trigger_invert{false};
 
-	uint32_t _triggers[num_gpios] {};
-	uint32_t _allocated_channels{0};
+    uint32_t _triggers[num_gpios]{};
+    uint32_t _allocated_channels{0};
 };
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.cpp b/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.cpp
index 9b54dd7b0b..5f733ea945 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.cpp
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.cpp
@@ -1,123 +1,94 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifdef __PX4_NUTTX
 
-#include <sys/ioctl.h>
-#include <lib/mathlib/mathlib.h>
-#include <parameters/param.h>
+#   include <sys/ioctl.h>
+#   include <lib/mathlib/mathlib.h>
+#   include <parameters/param.h>
 
 
-#include "drivers/drv_pwm_trigger.h"
-#include "pwm.h"
+#   include "drivers/drv_pwm_trigger.h"
+#   include "pwm.h"
 
-CameraInterfacePWM::CameraInterfacePWM():
-	CameraInterface()
-{
-	param_get(param_find("TRIG_PWM_SHOOT"), &_pwm_camera_shoot);
-	param_get(param_find("TRIG_PWM_NEUTRAL"), &_pwm_camera_neutral);
-	get_pins();
-	setup();
+CameraInterfacePWM::CameraInterfacePWM() :
+    CameraInterface() {
+    param_get(param_find("TRIG_PWM_SHOOT"), &_pwm_camera_shoot);
+    param_get(param_find("TRIG_PWM_NEUTRAL"), &_pwm_camera_neutral);
+    get_pins();
+    setup();
 }
 
-CameraInterfacePWM::~CameraInterfacePWM()
-{
-	// Deinitialise trigger channels
-	up_pwm_trigger_deinit();
+CameraInterfacePWM::~CameraInterfacePWM() {
+    // Deinitialise trigger channels
+    up_pwm_trigger_deinit();
 }
 
-void CameraInterfacePWM::setup()
-{
-	// Precompute the bitmask for enabled channels
-	uint32_t pin_bitmask = 0;
-
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		if (_pins[i] >= 0) {
-			pin_bitmask |= (1 << _pins[i]);
-		}
-	}
-
-	// Initialize and arm channels
-	int ret = up_pwm_trigger_init(pin_bitmask);
-
-	if (ret < 0) {
-		PX4_ERR("up_pwm_trigger_init failed (%i)", ret);
-		pin_bitmask = 0;
-
-	} else {
-		pin_bitmask = ret;
-	}
-
-	// Clear pins that could not be initialized
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		if (_pins[i] >= 0 && ((1 << _pins[i]) & pin_bitmask) == 0) {
-			_pins[i] = -1;
-		}
-	}
-
-	// Set neutral pulsewidths
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		if (_pins[i] >= 0) {
-			up_pwm_trigger_set(_pins[i], math::constrain(_pwm_camera_neutral, (int32_t) 0, (int32_t) 2000));
-		}
-	}
-
+void CameraInterfacePWM::setup() {
+    // Precompute the bitmask for enabled channels
+    uint32_t pin_bitmask = 0;
+
+    for (unsigned i = 0; i < arraySize(_pins); i++) {
+        if (_pins[i] >= 0) {
+            pin_bitmask |= (1 << _pins[i]);
+        }
+    }
+
+    // Initialize and arm channels
+    int ret = up_pwm_trigger_init(pin_bitmask);
+
+    if (ret < 0) {
+        PX4_ERR("up_pwm_trigger_init failed (%i)", ret);
+        pin_bitmask = 0;
+
+    } else {
+        pin_bitmask = ret;
+    }
+
+    // Clear pins that could not be initialized
+    for (unsigned i = 0; i < arraySize(_pins); i++) {
+        if (_pins[i] >= 0 && ((1 << _pins[i]) & pin_bitmask) == 0) {
+            _pins[i] = -1;
+        }
+    }
+
+    // Set neutral pulsewidths
+    for (unsigned i = 0; i < arraySize(_pins); i++) {
+        if (_pins[i] >= 0) {
+            up_pwm_trigger_set(_pins[i], math::constrain(_pwm_camera_neutral, (int32_t)0, (int32_t)2000));
+        }
+    }
 }
 
-void CameraInterfacePWM::trigger(bool trigger_on_true)
-{
-	for (unsigned i = 0; i < arraySize(_pins); i++) {
-		if (_pins[i] >= 0) {
-			// Set all valid pins to shoot or neutral levels
-			up_pwm_trigger_set(_pins[i], math::constrain(trigger_on_true ? _pwm_camera_shoot : _pwm_camera_neutral, (int32_t) 0,
-					   (int32_t) 2000));
-		}
-	}
+void CameraInterfacePWM::trigger(bool trigger_on_true) {
+    for (unsigned i = 0; i < arraySize(_pins); i++) {
+        if (_pins[i] >= 0) {
+            // Set all valid pins to shoot or neutral levels
+            up_pwm_trigger_set(_pins[i], math::constrain(trigger_on_true ? _pwm_camera_shoot : _pwm_camera_neutral, (int32_t)0,
+                                                         (int32_t)2000));
+        }
+    }
 }
 
-void CameraInterfacePWM::info()
-{
-	PX4_INFO_RAW("PWM trigger mode, pins enabled: ");
+void CameraInterfacePWM::info() {
+    PX4_INFO_RAW("PWM trigger mode, pins enabled: ");
 
-	for (unsigned i = 0; i < arraySize(_pins); ++i) {
-		if (_pins[i] < 0) {
-			continue;
-		}
+    for (unsigned i = 0; i < arraySize(_pins); ++i) {
+        if (_pins[i] < 0) {
+            continue;
+        }
 
-		PX4_INFO_RAW("[%d]", _pins[i] + 1);
-	}
+        PX4_INFO_RAW("[%d]", _pins[i] + 1);
+    }
 
-	PX4_INFO_RAW("\n");
+    PX4_INFO_RAW("\n");
 }
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.h b/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.h
index 6ef90320aa..48593c4fd9 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.h
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/pwm.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file pwm.h
@@ -41,27 +18,26 @@
 
 #ifdef __PX4_NUTTX
 
-#include <drivers/drv_hrt.h>
-#include <parameters/param.h>
-#include <px4_platform_common/log.h>
+#   include <drivers/drv_hrt.h>
+#   include <parameters/param.h>
+#   include <px4_platform_common/log.h>
 
-#include "camera_interface.h"
+#   include "camera_interface.h"
 
-class CameraInterfacePWM : public CameraInterface
-{
+class CameraInterfacePWM : public CameraInterface {
 public:
-	CameraInterfacePWM();
-	virtual ~CameraInterfacePWM();
+    CameraInterfacePWM();
+    virtual ~CameraInterfacePWM();
 
-	void trigger(bool trigger_on_true);
+    void trigger(bool trigger_on_true);
 
-	void info();
+    void info();
 
 private:
-	void setup();
+    void setup();
 
-	int32_t _pwm_camera_shoot{0};
-	int32_t _pwm_camera_neutral{0};
+    int32_t _pwm_camera_shoot{0};
+    int32_t _pwm_camera_neutral{0};
 };
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.cpp b/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.cpp
index 0f8b5f534c..c147dc7fab 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.cpp
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.cpp
@@ -1,146 +1,115 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifdef __PX4_NUTTX
 
-#include <sys/ioctl.h>
-#include <lib/mathlib/mathlib.h>
+#   include <sys/ioctl.h>
+#   include <lib/mathlib/mathlib.h>
 
-#include "drivers/drv_pwm_trigger.h"
-#include "seagull_map2.h"
+#   include "drivers/drv_pwm_trigger.h"
+#   include "seagull_map2.h"
 
 // PWM levels of the interface to Seagull MAP 2 converter to
 // Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf)
-#define PWM_CAMERA_DISARMED			900
-#define PWM_CAMERA_NEUTRAL			1500
-#define PWM_1_CAMERA_ON				1100
-#define PWM_1_CAMERA_AUTOFOCUS_SHOOT	1300
-#define PWM_1_CAMERA_INSTANT_SHOOT	1700
-#define PWM_1_CAMERA_OFF				1900
-#define PWM_2_CAMERA_KEEP_ALIVE		1700
-#define PWM_2_CAMERA_ON_OFF			1900
-
-CameraInterfaceSeagull::CameraInterfaceSeagull()
-{
-	get_pins();
-	setup();
+#   define PWM_CAMERA_DISARMED          900
+#   define PWM_CAMERA_NEUTRAL           1500
+#   define PWM_1_CAMERA_ON              1100
+#   define PWM_1_CAMERA_AUTOFOCUS_SHOOT 1300
+#   define PWM_1_CAMERA_INSTANT_SHOOT   1700
+#   define PWM_1_CAMERA_OFF             1900
+#   define PWM_2_CAMERA_KEEP_ALIVE      1700
+#   define PWM_2_CAMERA_ON_OFF          1900
+
+CameraInterfaceSeagull::CameraInterfaceSeagull() {
+    get_pins();
+    setup();
 }
 
-CameraInterfaceSeagull::~CameraInterfaceSeagull()
-{
-	// Deinitialise trigger channels
-	up_pwm_trigger_deinit();
+CameraInterfaceSeagull::~CameraInterfaceSeagull() {
+    // Deinitialise trigger channels
+    up_pwm_trigger_deinit();
 }
 
-void CameraInterfaceSeagull::setup()
-{
-	for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
-		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
-
-			// Initialize the interface
-			uint32_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
-			int ret = up_pwm_trigger_init(pin_bitmask);
+void CameraInterfaceSeagull::setup() {
+    for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
+        if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+            // Initialize the interface
+            uint32_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
+            int      ret         = up_pwm_trigger_init(pin_bitmask);
 
-			if (ret != (int)pin_bitmask) {
-				PX4_WARN("up_pwm_trigger_init failed (%i)", ret);
-				continue;
-			}
+            if (ret != (int)pin_bitmask) {
+                PX4_WARN("up_pwm_trigger_init failed (%i)", ret);
+                continue;
+            }
 
-			// Set both interface pins to disarmed
-			int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
-			PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i + 1, _pins[i + 1], PWM_CAMERA_DISARMED, ret1);
+            // Set both interface pins to disarmed
+            int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
+            PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i + 1, _pins[i + 1], PWM_CAMERA_DISARMED, ret1);
 
-			int ret2 = up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
-			PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i, _pins[i], PWM_CAMERA_DISARMED, ret2);
+            int ret2 = up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
+            PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i, _pins[i], PWM_CAMERA_DISARMED, ret2);
 
-			// We only support 2 consecutive pins while using the Seagull MAP2
-			return;
-		}
-	}
+            // We only support 2 consecutive pins while using the Seagull MAP2
+            return;
+        }
+    }
 
-	PX4_ERR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control.");
+    PX4_ERR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control.");
 }
 
-void CameraInterfaceSeagull::trigger(bool trigger_on_true)
-{
-	if (!_camera_is_on) {
-		return;
-	}
-
-	for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
-		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
-			// Set channel 1 to shoot or neutral levels
-			up_pwm_trigger_set(_pins[i + 1], trigger_on_true ? PWM_1_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL);
-		}
-	}
+void CameraInterfaceSeagull::trigger(bool trigger_on_true) {
+    if (!_camera_is_on) {
+        return;
+    }
+
+    for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
+        if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+            // Set channel 1 to shoot or neutral levels
+            up_pwm_trigger_set(_pins[i + 1], trigger_on_true ? PWM_1_CAMERA_INSTANT_SHOOT : PWM_CAMERA_NEUTRAL);
+        }
+    }
 }
 
-void CameraInterfaceSeagull::send_keep_alive(bool enable)
-{
-	// This should alternate between enable and !enable to keep the camera alive
-	if (!_camera_is_on) {
-		return;
-	}
-
-	for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
-		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
-			// Set channel 2 pin to keep_alive or netural signal
-			up_pwm_trigger_set(_pins[i], enable ? PWM_2_CAMERA_KEEP_ALIVE : PWM_CAMERA_NEUTRAL);
-		}
-	}
+void CameraInterfaceSeagull::send_keep_alive(bool enable) {
+    // This should alternate between enable and !enable to keep the camera alive
+    if (!_camera_is_on) {
+        return;
+    }
+
+    for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
+        if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+            // Set channel 2 pin to keep_alive or netural signal
+            up_pwm_trigger_set(_pins[i], enable ? PWM_2_CAMERA_KEEP_ALIVE : PWM_CAMERA_NEUTRAL);
+        }
+    }
 }
 
-void CameraInterfaceSeagull::send_toggle_power(bool enable)
-{
-	// This should alternate between enable and !enable to toggle camera power
-	for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
-		if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
-			// Set channel 1 to neutral
-			up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_NEUTRAL);
-			// Set channel 2 to on_off or neutral signal
-			up_pwm_trigger_set(_pins[i], enable ? PWM_2_CAMERA_ON_OFF : PWM_CAMERA_NEUTRAL);
-		}
-	}
-
-	if (!enable) {
-		_camera_is_on = !_camera_is_on;
-	}
+void CameraInterfaceSeagull::send_toggle_power(bool enable) {
+    // This should alternate between enable and !enable to toggle camera power
+    for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
+        if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
+            // Set channel 1 to neutral
+            up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_NEUTRAL);
+            // Set channel 2 to on_off or neutral signal
+            up_pwm_trigger_set(_pins[i], enable ? PWM_2_CAMERA_ON_OFF : PWM_CAMERA_NEUTRAL);
+        }
+    }
+
+    if (!enable) {
+        _camera_is_on = !_camera_is_on;
+    }
 }
 
-void CameraInterfaceSeagull::info()
-{
-	PX4_INFO("PWM trigger mode (Seagull MAP2) , pins enabled : [%d][%d][%d][%d][%d][%d]",
-		 _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]);
+void CameraInterfaceSeagull::info() {
+    PX4_INFO("PWM trigger mode (Seagull MAP2) , pins enabled : [%d][%d][%d][%d][%d][%d]",
+             _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]);
 }
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.h b/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.h
index f069bc88ea..281550533a 100644
--- a/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.h
+++ b/apps/peripheral/payload/camera_trigger/interfaces/src/seagull_map2.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2015-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file seagull_map2.h
@@ -41,33 +18,35 @@
 
 #ifdef __PX4_NUTTX
 
-#include <drivers/drv_hrt.h>
+#   include <drivers/drv_hrt.h>
 
-#include "camera_interface.h"
+#   include "camera_interface.h"
 
-class CameraInterfaceSeagull : public CameraInterface
-{
+class CameraInterfaceSeagull : public CameraInterface {
 public:
-	CameraInterfaceSeagull();
-	virtual ~CameraInterfaceSeagull();
+    CameraInterfaceSeagull();
+    virtual ~CameraInterfaceSeagull();
 
-	void trigger(bool trigger_on_true);
+    void trigger(bool trigger_on_true);
 
-	void send_keep_alive(bool enable);
+    void send_keep_alive(bool enable);
 
-	void send_toggle_power(bool enable);
+    void send_toggle_power(bool enable);
 
-	void info();
+    void info();
 
-	bool has_power_control() { return true; }
+    bool has_power_control() {
+        return true;
+    }
 
-	bool is_powered_on() { return _camera_is_on; }
+    bool is_powered_on() {
+        return _camera_is_on;
+    }
 
 private:
-	void setup();
-
-	bool _camera_is_on{false};
+    void setup();
 
+    bool _camera_is_on{false};
 };
 
 #endif /* ifdef __PX4_NUTTX */
diff --git a/apps/peripheral/radio_control/rc_input/RCInput.cpp b/apps/peripheral/radio_control/rc_input/RCInput.cpp
index 80c7d7afd1..cc8426171b 100644
--- a/apps/peripheral/radio_control/rc_input/RCInput.cpp
+++ b/apps/peripheral/radio_control/rc_input/RCInput.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "RCInput.hpp"
 
@@ -43,886 +20,852 @@ using namespace time_literals;
 constexpr char const *RCInput::RC_SCAN_STRING[];
 
 RCInput::RCInput(const char *device) :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
-	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
-{
-	// initialize raw_rc values and count
-	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	if (device) {
-		strncpy(_device, device, sizeof(_device) - 1);
-		_device[sizeof(_device) - 1] = '\0';
-	}
-
-	if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
-		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
-	}
+    ModuleParams(nullptr),
+    ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
+    _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle time")),
+    _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME ": publish interval")) {
+    // initialize raw_rc values and count
+    for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
+        _raw_rc_values[i] = UINT16_MAX;
+    }
+
+    if (device) {
+        strncpy(_device, device, sizeof(_device) - 1);
+        _device[sizeof(_device) - 1] = '\0';
+    }
+
+    if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+        _rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+    }
 }
 
-RCInput::~RCInput()
-{
+RCInput::~RCInput() {
 #if defined(SPEKTRUM_POWER_PASSIVE)
-	// Disable power controls for Spektrum receiver
-	SPEKTRUM_POWER_PASSIVE();
+    // Disable power controls for Spektrum receiver
+    SPEKTRUM_POWER_PASSIVE();
 #endif
-	dsm_deinit();
+    dsm_deinit();
 
-	delete _crsf_telemetry;
-	delete _ghst_telemetry;
+    delete _crsf_telemetry;
+    delete _ghst_telemetry;
 
-	perf_free(_cycle_perf);
-	perf_free(_publish_interval_perf);
+    perf_free(_cycle_perf);
+    perf_free(_publish_interval_perf);
 }
 
-int
-RCInput::init()
-{
+int RCInput::init() {
 #ifdef RF_RADIO_POWER_CONTROL
-	// power radio on
-	RF_RADIO_POWER_CONTROL(true);
+    // power radio on
+    RF_RADIO_POWER_CONTROL(true);
 #endif // RF_RADIO_POWER_CONTROL
 
-	// dsm_init sets some file static variables and returns a file descriptor
-	// it also powers on the radio if needed
-	_rcs_fd = dsm_init(_device);
+    // dsm_init sets some file static variables and returns a file descriptor
+    // it also powers on the radio if needed
+    _rcs_fd = dsm_init(_device);
 
-	if (_rcs_fd < 0) {
-		return -errno;
-	}
+    if (_rcs_fd < 0) {
+        return -errno;
+    }
 
-	if (board_rc_swap_rxtx(_device)) {
+    if (board_rc_swap_rxtx(_device)) {
 #if defined(TIOCSSWAP)
-		ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+        ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
 #endif // TIOCSSWAP
-	}
+    }
 
-	// assume SBUS input and immediately switch it to
-	// so that if Single wire mode on TX there will be only
-	// a short contention
-	sbus_config(_rcs_fd, board_rc_singlewire(_device));
+    // assume SBUS input and immediately switch it to
+    // so that if Single wire mode on TX there will be only
+    // a short contention
+    sbus_config(_rcs_fd, board_rc_singlewire(_device));
 
 #ifdef GPIO_PPM_IN
-	// disable CPPM input by mapping it away from the timer capture input
-	px4_arch_unconfiggpio(GPIO_PPM_IN);
+    // disable CPPM input by mapping it away from the timer capture input
+    px4_arch_unconfiggpio(GPIO_PPM_IN);
 #endif // GPIO_PPM_IN
 
-	rc_io_invert(false);
+    rc_io_invert(false);
 
-	return 0;
+    return 0;
 }
 
-int
-RCInput::task_spawn(int argc, char *argv[])
-{
-	bool error_flag = false;
+int RCInput::task_spawn(int argc, char *argv[]) {
+    bool error_flag = false;
 
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-	const char *device_name = nullptr;
+    int         myoptind = 1;
+    int         ch;
+    const char *myoptarg    = nullptr;
+    const char *device_name = nullptr;
 #if defined(RC_SERIAL_PORT)
-	device_name = RC_SERIAL_PORT;
+    device_name = RC_SERIAL_PORT;
 #endif // RC_SERIAL_PORT
 
 #if defined(RC_SERIAL_PORT) && defined(PX4IO_SERIAL_DEVICE)
 
-	// if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running
-	if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) {
-		device_name = nullptr;
-	}
+    // if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running
+    if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) {
+        device_name = nullptr;
+    }
 
 #endif // RC_SERIAL_PORT && PX4IO_SERIAL_DEVICE
 
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'd':
-			device_name = myoptarg;
-			break;
+    while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
+        switch (ch) {
+        case 'd':
+            device_name = myoptarg;
+            break;
 
-		case '?':
-			error_flag = true;
-			break;
+        case '?':
+            error_flag = true;
+            break;
 
-		default:
-			PX4_WARN("unrecognized flag");
-			error_flag = true;
-			break;
-		}
-	}
+        default:
+            PX4_WARN("unrecognized flag");
+            error_flag = true;
+            break;
+        }
+    }
 
-	if (error_flag) {
-		return -1;
-	}
+    if (error_flag) {
+        return -1;
+    }
 
-	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
-		RCInput *instance = new RCInput(device_name);
+    if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+        RCInput *instance = new RCInput(device_name);
 
-		if (instance == nullptr) {
-			PX4_ERR("alloc failed");
-			return PX4_ERROR;
-		}
+        if (instance == nullptr) {
+            PX4_ERR("alloc failed");
+            return PX4_ERROR;
+        }
 
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
+        _object.store(instance);
+        _task_id = task_id_is_work_queue;
 
-		instance->ScheduleOnInterval(_current_update_interval);
+        instance->ScheduleOnInterval(_current_update_interval);
 
-		return PX4_OK;
+        return PX4_OK;
 
-	} else {
-		if (device_name) {
-			PX4_ERR("invalid device (-d) %s", device_name);
+    } else {
+        if (device_name) {
+            PX4_ERR("invalid device (-d) %s", device_name);
 
-		} else {
-			PX4_INFO("valid device required");
-		}
-	}
+        } else {
+            PX4_INFO("valid device required");
+        }
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void
-RCInput::fill_rc_in(uint16_t raw_rc_count_local,
-		    uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-		    hrt_abstime now, bool frame_drop, bool failsafe,
-		    unsigned frame_drops, int rssi = -1)
-{
-	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
-
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
-	}
-
-	unsigned valid_chans = 0;
-
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
-
-		if (raw_rc_values_local[i] != UINT16_MAX) {
-			valid_chans++;
-		}
-
-		// once filled, reset values back to default
-		_raw_rc_values[i] = UINT16_MAX;
-	}
-
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
-
-	/* fake rssi if no value was provided */
-	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
-			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
-			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
-			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
-
-			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
-
-		} else if (_analog_rc_rssi_stable) {
-			// set RSSI if analog RSSI input is present
-			float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
-
-			if (rssi_analog > 100.0f) {
-				rssi_analog = 100.0f;
-			}
-
-			if (rssi_analog < 0.0f) {
-				rssi_analog = 0.0f;
-			}
-
-			_rc_in.rssi = rssi_analog;
-
-		} else {
-			_rc_in.rssi = 255;
-		}
-
-	} else {
-		_rc_in.rssi = rssi;
-	}
-
-	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
-	}
-
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
-}
+void RCInput::fill_rc_in(uint16_t    raw_rc_count_local,
+                         uint16_t    raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+                         hrt_abstime now, bool frame_drop, bool failsafe,
+                         unsigned frame_drops, int rssi = -1) {
+    // fill rc_in struct for publishing
+    _rc_in.channel_count = raw_rc_count_local;
 
-void RCInput::set_rc_scan_state(RC_SCAN newState)
-{
-	if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE)
-	    && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+    if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+        _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+    }
 
-		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+    unsigned valid_chans = 0;
 
-	} else if (_param_rc_input_proto.get() < 0) {
-		// only auto change if RC_INPUT_PROTO set to auto (-1)
-		PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
-		_rc_scan_state = newState;
+    for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+        _rc_in.values[i] = raw_rc_values_local[i];
 
-	} else {
-		_rc_scan_state = RC_SCAN::RC_SCAN_NONE;
-	}
+        if (raw_rc_values_local[i] != UINT16_MAX) {
+            valid_chans++;
+        }
 
-	_rc_scan_begin = 0;
-	_rc_scan_locked = false;
-}
+        // once filled, reset values back to default
+        _raw_rc_values[i] = UINT16_MAX;
+    }
 
-void RCInput::rc_io_invert(bool invert)
-{
-	// First check if the board provides a board-specific inversion method (e.g. via GPIO),
-	// and if not use an IOCTL
-	if (!board_rc_invert_input(_device, invert)) {
-#if defined(TIOCSINVERT)
+    _rc_in.timestamp             = now;
+    _rc_in.timestamp_last_signal = _rc_in.timestamp;
+    _rc_in.rc_ppm_frame_length   = 0;
 
-		if (invert) {
-			ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
+    /* fake rssi if no value was provided */
+    if (rssi == -1) {
+        if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+            const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
+            const int32_t rssi_pwm_min  = _param_rc_rssi_pwm_min.get();
+            const int32_t rssi_pwm_max  = _param_rc_rssi_pwm_max.get();
 
-		} else {
-			ioctl(_rcs_fd, TIOCSINVERT, 0);
-		}
+            // get RSSI from input channel
+            int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+            _rc_in.rssi = math::constrain(rc_rssi, 0, 100);
 
-#endif // TIOCSINVERT
-	}
-}
+        } else if (_analog_rc_rssi_stable) {
+            // set RSSI if analog RSSI input is present
+            float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
 
-void RCInput::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
+            if (rssi_analog > 100.0f) {
+                rssi_analog = 100.0f;
+            }
 
-	if (!_initialized) {
-		if (init() == PX4_OK) {
-			_initialized = true;
+            if (rssi_analog < 0.0f) {
+                rssi_analog = 0.0f;
+            }
 
-		} else {
-			PX4_ERR("init failed");
-			exit_and_cleanup();
-		}
+            _rc_in.rssi = rssi_analog;
 
-	} else {
+        } else {
+            _rc_in.rssi = 255;
+        }
 
-		perf_begin(_cycle_perf);
+    } else {
+        _rc_in.rssi = rssi;
+    }
 
-		// Check if parameters have changed
-		if (_parameter_update_sub.updated()) {
-			// clear update
-			parameter_update_s param_update;
-			_parameter_update_sub.copy(&param_update);
+    if (valid_chans == 0) {
+        _rc_in.rssi = 0;
+    }
 
-			updateParams();
-		}
+    _rc_in.rc_failsafe          = failsafe;
+    _rc_in.rc_lost              = (valid_chans == 0);
+    _rc_in.rc_lost_frame_count  = frame_drops;
+    _rc_in.rc_total_frame_count = 0;
+}
 
-		if (_vehicle_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
+void RCInput::set_rc_scan_state(RC_SCAN newState) {
+    if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE)
+        && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+        _rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
 
-			if (_vehicle_status_sub.copy(&vehicle_status)) {
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			}
-		}
+    } else if (_param_rc_input_proto.get() < 0) {
+        // only auto change if RC_INPUT_PROTO set to auto (-1)
+        PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+        _rc_scan_state = newState;
 
-		const hrt_abstime cycle_timestamp = hrt_absolute_time();
+    } else {
+        _rc_scan_state = RC_SCAN::RC_SCAN_NONE;
+    }
 
+    _rc_scan_begin  = 0;
+    _rc_scan_locked = false;
+}
 
-		/* vehicle command */
-		vehicle_command_s vcmd;
+void RCInput::rc_io_invert(bool invert) {
+    // First check if the board provides a board-specific inversion method (e.g. via GPIO),
+    // and if not use an IOCTL
+    if (!board_rc_invert_input(_device, invert)) {
+#if defined(TIOCSINVERT)
 
-		if (_vehicle_cmd_sub.update(&vcmd)) {
-			// Check for a pairing command
-			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
+        if (invert) {
+            ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
 
-				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-#if defined(SPEKTRUM_POWER)
+        } else {
+            ioctl(_rcs_fd, TIOCSINVERT, 0);
+        }
 
-				if (!_rc_scan_locked && !_armed) {
-					if ((int)vcmd.param1 == 0) {
-						// DSM binding command
-						int dsm_bind_mode = (int)vcmd.param2;
+#endif // TIOCSINVERT
+    }
+}
 
-						int dsm_bind_pulses = 0;
+void RCInput::Run() {
+    if (should_exit()) {
+        exit_and_cleanup();
+        return;
+    }
 
-						if (dsm_bind_mode == 0) {
-							dsm_bind_pulses = DSM2_BIND_PULSES;
+    if (!_initialized) {
+        if (init() == PX4_OK) {
+            _initialized = true;
 
-						} else if (dsm_bind_mode == 1) {
-							dsm_bind_pulses = DSMX_BIND_PULSES;
+        } else {
+            PX4_ERR("init failed");
+            exit_and_cleanup();
+        }
 
-						} else {
-							dsm_bind_pulses = DSMX8_BIND_PULSES;
-						}
+    } else {
+        perf_begin(_cycle_perf);
 
-						bind_spektrum(dsm_bind_pulses);
+        // Check if parameters have changed
+        if (_parameter_update_sub.updated()) {
+            // clear update
+            parameter_update_s param_update;
+            _parameter_update_sub.copy(&param_update);
 
-						cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-					}
+            updateParams();
+        }
 
-				} else {
-					cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
+        if (_vehicle_status_sub.updated()) {
+            vehicle_status_s vehicle_status;
 
-#endif // SPEKTRUM_POWER
+            if (_vehicle_status_sub.copy(&vehicle_status)) {
+                _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+            }
+        }
 
-				// publish acknowledgement
-				vehicle_command_ack_s command_ack{};
-				command_ack.command = vcmd.command;
-				command_ack.result = cmd_ret;
-				command_ack.target_system = vcmd.source_system;
-				command_ack.target_component = vcmd.source_component;
-				command_ack.timestamp = hrt_absolute_time();
-				uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
-				vehicle_command_ack_pub.publish(command_ack);
-			}
-		}
+        const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
 
-#if defined(ADC_RC_RSSI_CHANNEL)
+        /* vehicle command */
+        vehicle_command_s vcmd;
 
-		// update ADC sampling
-		if (_adc_report_sub.updated()) {
-			adc_report_s adc;
+        if (_vehicle_cmd_sub.update(&vcmd)) {
+            // Check for a pairing command
+            if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
+                uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+#if defined(SPEKTRUM_POWER)
 
-			if (_adc_report_sub.copy(&adc)) {
-				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
-					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-						float adc_volt = adc.raw_data[i] *
-								 adc.v_ref /
-								 adc.resolution;
+                if (!_rc_scan_locked && !_armed) {
+                    if ((int)vcmd.param1 == 0) {
+                        // DSM binding command
+                        int dsm_bind_mode = (int)vcmd.param2;
 
-						if (_analog_rc_rssi_volt < 0.0f) {
-							_analog_rc_rssi_volt = adc_volt;
-						}
+                        int dsm_bind_pulses = 0;
 
-						_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
+                        if (dsm_bind_mode == 0) {
+                            dsm_bind_pulses = DSM2_BIND_PULSES;
 
-						/* only allow this to be used if we see a high RSSI once */
-						if (_analog_rc_rssi_volt > 2.5f) {
-							_analog_rc_rssi_stable = true;
-						}
-					}
-				}
-			}
-		}
+                        } else if (dsm_bind_mode == 1) {
+                            dsm_bind_pulses = DSMX_BIND_PULSES;
 
-#endif // ADC_RC_RSSI_CHANNEL
+                        } else {
+                            dsm_bind_pulses = DSMX8_BIND_PULSES;
+                        }
 
-		bool rc_updated = false;
-
-		// This block scans for a supported serial RC input and locks onto the first one found
-		// Scan for 500 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 500_ms;
-
-		unsigned frame_drops = 0;
-
-		// TODO: needs work (poll _rcs_fd)
-		// int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
-		// then update priority to SCHED_PRIORITY_FAST_DRIVER
-		// read all available data from the serial RC input UART
-
-		// read all available data from the serial RC input UART
-		int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
-
-		if (newBytes > 0) {
-			_bytes_rx += newBytes;
-		}
-
-		const bool rc_scan_locked = _rc_scan_locked;
+                        bind_spektrum(dsm_bind_pulses);
 
-		switch (_rc_scan_state) {
-		case RC_SCAN_NONE:
-			// do nothing
-			break;
-
-		case RC_SCAN_SBUS:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for SBUS
-				sbus_config(_rcs_fd, board_rc_singlewire(_device));
-				rc_io_invert(true);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					bool sbus_failsafe = false;
-					bool sbus_frame_drop = false;
-
-					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
-								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   sbus_frame_drop, sbus_failsafe, frame_drops);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				rc_io_invert(false);
-				set_rc_scan_state(RC_SCAN_DSM);
-			}
-
-			break;
-
-		case RC_SCAN_DSM:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					int8_t dsm_rssi = 0;
-					bool dsm_11_bit = false;
-
-					// parse new data
-					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
-							       &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, false, frame_drops, dsm_rssi);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_ST24);
-			}
-
-			break;
-
-		case RC_SCAN_ST24:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					// parse new data
-					uint8_t st24_rssi, lost_count;
-
-					rc_updated = false;
-
-					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
-						st24_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
-					}
-
-					// The st24 will keep outputting RC channels and RSSI even if RC has been lost.
-					// The only way to detect RC loss is therefore to look at the lost_count.
-
-					if (rc_updated) {
-						if (lost_count == 0) {
-							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
-							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-								   false, false, frame_drops, st24_rssi);
-							_rc_scan_locked = true;
-
-						} else {
-							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
-						}
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SUMD);
-			}
-
-			break;
-
-		case RC_SCAN_SUMD:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for DSM
-				dsm_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				if (newBytes > 0) {
-					// parse new data
-					uint8_t sumd_rssi, rx_count;
-					bool sumd_failsafe;
-
-					rc_updated = false;
-
-					for (unsigned i = 0; i < (unsigned)newBytes; i++) {
-						/* set updated flag if one complete packet was parsed */
-						sumd_rssi = input_rc_s::RSSI_MAX;
-						rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
-										&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
-					}
-
-					if (rc_updated) {
-						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
-							   false, sumd_failsafe, frame_drops, sumd_rssi);
-						_rc_scan_locked = true;
-					}
-				}
-
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_PPM);
-			}
-
-			break;
-
-		case RC_SCAN_PPM:
-			// skip PPM if it's not supported
-#ifdef HRT_PPM_CHANNEL
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure timer input pin for CPPM
-				px4_arch_configgpio(GPIO_PPM_IN);
-
-			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
-					// we have a new PPM frame. Publish it.
-					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
-					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
-				}
-
-			} else {
-				// disable CPPM input by mapping it away from the timer capture input
-				px4_arch_unconfiggpio(GPIO_PPM_IN);
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_CRSF);
-			}
-
-#else   // skip PPM if it's not supported
-			set_rc_scan_state(RC_SCAN_CRSF);
-
-#endif  // HRT_PPM_CHANNEL
-
-			break;
-
-		case RC_SCAN_CRSF:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for CRSF
-				crsf_config(_rcs_fd);
-
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
-
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
-
-				// parse new data
-				if (newBytes > 0) {
-					rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
-								input_rc_s::RC_INPUT_MAX_CHANNELS);
-
-					if (rc_updated) {
-						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
-
-						// on Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
-#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
+                        cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+                    }
 
-						if (!_rc_scan_locked && !_crsf_telemetry) {
-							_crsf_telemetry = new CRSFTelemetry(_rcs_fd);
-						}
+                } else {
+                    cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+                }
 
-#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
+#endif // SPEKTRUM_POWER
+
+                // publish acknowledgement
+                vehicle_command_ack_s command_ack{};
+                command_ack.command          = vcmd.command;
+                command_ack.result           = cmd_ret;
+                command_ack.target_system    = vcmd.source_system;
+                command_ack.target_component = vcmd.source_component;
+                command_ack.timestamp        = hrt_absolute_time();
+                uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+                vehicle_command_ack_pub.publish(command_ack);
+            }
+        }
 
-						_rc_scan_locked = true;
 
-						if (_crsf_telemetry) {
-							_crsf_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
+#if defined(ADC_RC_RSSI_CHANNEL)
+
+        // update ADC sampling
+        if (_adc_report_sub.updated()) {
+            adc_report_s adc;
 
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_GHST);
-			}
+            if (_adc_report_sub.copy(&adc)) {
+                for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
+                    if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
+                        float adc_volt = adc.raw_data[i] * adc.v_ref / adc.resolution;
 
-			break;
+                        if (_analog_rc_rssi_volt < 0.0f) {
+                            _analog_rc_rssi_volt = adc_volt;
+                        }
 
-		case RC_SCAN_GHST:
-			if (_rc_scan_begin == 0) {
-				_rc_scan_begin = cycle_timestamp;
-				// Configure serial port for GHST
-				ghst_config(_rcs_fd);
+                        _analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
 
-				// flush serial buffer and any existing buffered data
-				tcflush(_rcs_fd, TCIOFLUSH);
-				memset(_rcs_buf, 0, sizeof(_rcs_buf));
+                        /* only allow this to be used if we see a high RSSI once */
+                        if (_analog_rc_rssi_volt > 2.5f) {
+                            _analog_rc_rssi_stable = true;
+                        }
+                    }
+                }
+            }
+        }
+
+#endif // ADC_RC_RSSI_CHANNEL
 
-			} else if (_rc_scan_locked
-				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+        bool rc_updated = false;
+
+        // This block scans for a supported serial RC input and locks onto the first one found
+        // Scan for 500 msec, then switch protocol
+        constexpr hrt_abstime rc_scan_max = 500_ms;
+
+        unsigned frame_drops = 0;
+
+        // TODO: needs work (poll _rcs_fd)
+        // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100);
+        // then update priority to SCHED_PRIORITY_FAST_DRIVER
+        // read all available data from the serial RC input UART
+
+        // read all available data from the serial RC input UART
+        int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
+
+        if (newBytes > 0) {
+            _bytes_rx += newBytes;
+        }
+
+        const bool rc_scan_locked = _rc_scan_locked;
+
+        switch (_rc_scan_state) {
+        case RC_SCAN_NONE:
+            // do nothing
+            break;
+
+        case RC_SCAN_SBUS:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for SBUS
+                sbus_config(_rcs_fd, board_rc_singlewire(_device));
+                rc_io_invert(true);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                // parse new data
+                if (newBytes > 0) {
+                    bool sbus_failsafe   = false;
+                    bool sbus_frame_drop = false;
+
+                    rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
+                                            &sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+                    if (rc_updated) {
+                        // we have a new SBUS frame. Publish it.
+                        _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+                        fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+                                   sbus_frame_drop, sbus_failsafe, frame_drops);
+                        _rc_scan_locked = true;
+                    }
+                }
+
+            } else {
+                // Scan the next protocol
+                rc_io_invert(false);
+                set_rc_scan_state(RC_SCAN_DSM);
+            }
+
+            break;
+
+        case RC_SCAN_DSM:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for DSM
+                dsm_config(_rcs_fd);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                if (newBytes > 0) {
+                    int8_t dsm_rssi   = 0;
+                    bool   dsm_11_bit = false;
+
+                    // parse new data
+                    rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+                                           &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+                    if (rc_updated) {
+                        // we have a new DSM frame. Publish it.
+                        _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+                        fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+                                   false, false, frame_drops, dsm_rssi);
+                        _rc_scan_locked = true;
+                    }
+                }
+
+            } else {
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_ST24);
+            }
+
+            break;
+
+        case RC_SCAN_ST24:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for DSM
+                dsm_config(_rcs_fd);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                if (newBytes > 0) {
+                    // parse new data
+                    uint8_t st24_rssi, lost_count;
+
+                    rc_updated = false;
+
+                    for (unsigned i = 0; i < (unsigned)newBytes; i++) {
+                        /* set updated flag if one complete packet was parsed */
+                        st24_rssi  = input_rc_s::RSSI_MAX;
+                        rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
+                    }
+
+                    // The st24 will keep outputting RC channels and RSSI even if RC has been lost.
+                    // The only way to detect RC loss is therefore to look at the lost_count.
+
+                    if (rc_updated) {
+                        if (lost_count == 0) {
+                            // we have a new ST24 frame. Publish it.
+                            _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
+                            fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+                                       false, false, frame_drops, st24_rssi);
+                            _rc_scan_locked = true;
+
+                        } else {
+                            // if the lost count > 0 means that there is an RC loss
+                            _rc_in.rc_lost = true;
+                        }
+                    }
+                }
+
+            } else {
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_SUMD);
+            }
+
+            break;
+
+        case RC_SCAN_SUMD:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for DSM
+                dsm_config(_rcs_fd);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                if (newBytes > 0) {
+                    // parse new data
+                    uint8_t sumd_rssi, rx_count;
+                    bool    sumd_failsafe;
+
+                    rc_updated = false;
+
+                    for (unsigned i = 0; i < (unsigned)newBytes; i++) {
+                        /* set updated flag if one complete packet was parsed */
+                        sumd_rssi  = input_rc_s::RSSI_MAX;
+                        rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
+                    }
+
+                    if (rc_updated) {
+                        // we have a new SUMD frame. Publish it.
+                        _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+                        fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
+                                   false, sumd_failsafe, frame_drops, sumd_rssi);
+                        _rc_scan_locked = true;
+                    }
+                }
+
+            } else {
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_PPM);
+            }
+
+            break;
+
+        case RC_SCAN_PPM:
+            // skip PPM if it's not supported
+#ifdef HRT_PPM_CHANNEL
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure timer input pin for CPPM
+                px4_arch_configgpio(GPIO_PPM_IN);
+
+            } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                // see if we have new PPM input data
+                if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
+                    // we have a new PPM frame. Publish it.
+                    rc_updated          = true;
+                    _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+                    fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
+                    _rc_scan_locked              = true;
+                    _rc_in.rc_ppm_frame_length   = ppm_frame_length;
+                    _rc_in.timestamp_last_signal = ppm_last_valid_decode;
+                }
+
+            } else {
+                // disable CPPM input by mapping it away from the timer capture input
+                px4_arch_unconfiggpio(GPIO_PPM_IN);
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_CRSF);
+            }
+
+#else  // skip PPM if it's not supported
+            set_rc_scan_state(RC_SCAN_CRSF);
+
+#endif // HRT_PPM_CHANNEL
+
+            break;
+
+        case RC_SCAN_CRSF:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for CRSF
+                crsf_config(_rcs_fd);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                // parse new data
+                if (newBytes > 0) {
+                    rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
+                                            input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+                    if (rc_updated) {
+                        // we have a new CRSF frame. Publish it.
+                        _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+                        fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
+
+                        // on Pixhawk (-related) boards we cannot write to the RC UART
+                        // another option is to use a different UART port
+#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
 
-				// parse new data
-				if (newBytes > 0) {
-					int8_t ghst_rssi = -1;
-					rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
-								&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+                        if (!_rc_scan_locked && !_crsf_telemetry) {
+                            _crsf_telemetry = new CRSFTelemetry(_rcs_fd);
+                        }
 
-					if (rc_updated) {
-						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
-						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
 
-						// ghst telemetry works on fmu-v5
-						// on other Pixhawk (-related) boards we cannot write to the RC UART
-						// another option is to use a different UART port
+                        _rc_scan_locked = true;
+
+                        if (_crsf_telemetry) {
+                            _crsf_telemetry->update(cycle_timestamp);
+                        }
+                    }
+                }
+
+            } else {
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_GHST);
+            }
+
+            break;
+
+        case RC_SCAN_GHST:
+            if (_rc_scan_begin == 0) {
+                _rc_scan_begin = cycle_timestamp;
+                // Configure serial port for GHST
+                ghst_config(_rcs_fd);
+
+                // flush serial buffer and any existing buffered data
+                tcflush(_rcs_fd, TCIOFLUSH);
+                memset(_rcs_buf, 0, sizeof(_rcs_buf));
+
+            } else if (_rc_scan_locked
+                       || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
+                // parse new data
+                if (newBytes > 0) {
+                    int8_t ghst_rssi = -1;
+                    rc_updated       = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
+                                                  &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+                    if (rc_updated) {
+                        // we have a new GHST frame. Publish it.
+                        _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+                        fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
+
+                        // ghst telemetry works on fmu-v5
+                        // on other Pixhawk (-related) boards we cannot write to the RC UART
+                        // another option is to use a different UART port
 #ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
 
-						if (!_rc_scan_locked && !_ghst_telemetry) {
-							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
-						}
+                        if (!_rc_scan_locked && !_ghst_telemetry) {
+                            _ghst_telemetry = new GHSTTelemetry(_rcs_fd);
+                        }
 
 #endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */
 
-						_rc_scan_locked = true;
+                        _rc_scan_locked = true;
 
-						if (_ghst_telemetry) {
-							_ghst_telemetry->update(cycle_timestamp);
-						}
-					}
-				}
+                        if (_ghst_telemetry) {
+                            _ghst_telemetry->update(cycle_timestamp);
+                        }
+                    }
+                }
 
-			} else {
-				// Scan the next protocol
-				set_rc_scan_state(RC_SCAN_SBUS);
-			}
+            } else {
+                // Scan the next protocol
+                set_rc_scan_state(RC_SCAN_SBUS);
+            }
 
-			break;
-		}
+            break;
+        }
 
-		perf_end(_cycle_perf);
+        perf_end(_cycle_perf);
 
-		if (rc_updated) {
-			perf_count(_publish_interval_perf);
+        if (rc_updated) {
+            perf_count(_publish_interval_perf);
 
-			_rc_in.link_quality = -1;
-			_rc_in.rssi_dbm = NAN;
+            _rc_in.link_quality = -1;
+            _rc_in.rssi_dbm     = NAN;
 
-			_to_input_rc.publish(_rc_in);
+            _to_input_rc.publish(_rc_in);
 
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
-			_rc_scan_locked = false;
-		}
+        } else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+            _rc_scan_locked = false;
+        }
 
-		if (!rc_scan_locked && _rc_scan_locked) {
-			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
-		}
+        if (!rc_scan_locked && _rc_scan_locked) {
+            PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
+        }
 
-		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
-		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
-		    && (_param_rc_input_proto.get() < 0)
-		   ) {
-			// RC_INPUT_PROTO auto => locked selection
-			_param_rc_input_proto.set(_rc_scan_state);
-			_param_rc_input_proto.commit();
-		}
-	}
+        // set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
+        if (!_armed && rc_updated && _rc_scan_locked
+            && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
+            && (_param_rc_input_proto.get() < 0)) {
+            // RC_INPUT_PROTO auto => locked selection
+            _param_rc_input_proto.set(_rc_scan_state);
+            _param_rc_input_proto.commit();
+        }
+    }
 }
 
 #if defined(SPEKTRUM_POWER)
-bool RCInput::bind_spektrum(int arg) const
-{
-	int ret = PX4_ERROR;
-
-	/* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */
+bool RCInput::bind_spektrum(int arg) const {
+    int ret = PX4_ERROR;
 
-	/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
-	PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8"));
+    /* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */
 
-	if (arg == DSM2_BIND_PULSES ||
-	    arg == DSMX_BIND_PULSES ||
-	    arg == DSMX8_BIND_PULSES) {
+    /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
+    PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8"));
 
-		dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
+    if (arg == DSM2_BIND_PULSES || arg == DSMX_BIND_PULSES || arg == DSMX8_BIND_PULSES) {
+        dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0);
 
-		dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
-		usleep(500000);
+        dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0);
+        usleep(500000);
 
-		dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
-		usleep(72000);
+        dsm_bind(DSM_CMD_BIND_POWER_UP, 0);
+        usleep(72000);
 
-		irqstate_t flags = px4_enter_critical_section();
-		dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg);
-		px4_leave_critical_section(flags);
+        irqstate_t flags = px4_enter_critical_section();
+        dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg);
+        px4_leave_critical_section(flags);
 
-		usleep(50000);
+        usleep(50000);
 
-		dsm_bind(DSM_CMD_BIND_REINIT_UART, 0);
+        dsm_bind(DSM_CMD_BIND_REINIT_UART, 0);
 
-		ret = OK;
+        ret = OK;
 
-	} else {
-		PX4_ERR("DSM bind failed");
-		ret = -EINVAL;
-	}
+    } else {
+        PX4_ERR("DSM bind failed");
+        ret = -EINVAL;
+    }
 
-	return (ret == PX4_OK);
+    return (ret == PX4_OK);
 }
 #endif /* SPEKTRUM_POWER */
 
-int RCInput::custom_command(int argc, char *argv[])
-{
+int RCInput::custom_command(int argc, char *argv[]) {
 #if defined(SPEKTRUM_POWER)
-	const char *verb = argv[0];
+    const char *verb = argv[0];
 
-	if (!strcmp(verb, "bind")) {
-		uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
-		vehicle_command_s vcmd{};
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
-		vcmd.timestamp = hrt_absolute_time();
-		vehicle_command_pub.publish(vcmd);
-		return 0;
-	}
+    if (!strcmp(verb, "bind")) {
+        uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+        vehicle_command_s                    vcmd{};
+        vcmd.command   = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
+        vcmd.timestamp = hrt_absolute_time();
+        vehicle_command_pub.publish(vcmd);
+        return 0;
+    }
 
 #endif /* SPEKTRUM_POWER */
 
-	/* start the FMU if not running */
-	if (!is_running()) {
-		int ret = RCInput::task_spawn(argc, argv);
+    /* start the FMU if not running */
+    if (!is_running()) {
+        int ret = RCInput::task_spawn(argc, argv);
 
-		if (ret) {
-			return ret;
-		}
-	}
+        if (ret) {
+            return ret;
+        }
+    }
 
-	return print_usage("unknown command");
+    return print_usage("unknown command");
 }
 
-int RCInput::print_status()
-{
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
+int RCInput::print_status() {
+    PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
 
-	if (_device[0] != '\0') {
-		PX4_INFO("UART device: %s", _device);
-		PX4_INFO("UART RX bytes: %"  PRIu32, _bytes_rx);
-	}
+    if (_device[0] != '\0') {
+        PX4_INFO("UART device: %s", _device);
+        PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx);
+    }
 
-	PX4_INFO("RC state: %s: %s", _rc_scan_locked ? "found" : "searching for signal", RC_SCAN_STRING[_rc_scan_state]);
+    PX4_INFO("RC state: %s: %s", _rc_scan_locked ? "found" : "searching for signal", RC_SCAN_STRING[_rc_scan_state]);
 
-	if (_rc_scan_locked) {
-		switch (_rc_scan_state) {
-		case RC_SCAN_NONE:
-			break;
+    if (_rc_scan_locked) {
+        switch (_rc_scan_state) {
+        case RC_SCAN_NONE:
+            break;
 
-		case RC_SCAN_CRSF:
-			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
-			break;
+        case RC_SCAN_CRSF:
+            PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
+            break;
 
-		case RC_SCAN_GHST:
-			PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
-			break;
+        case RC_SCAN_GHST:
+            PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no");
+            break;
 
-		case RC_SCAN_SBUS:
-			PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
-			break;
+        case RC_SCAN_SBUS:
+            PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames());
+            break;
 
 
-		case RC_SCAN_DSM:
-			// DSM status output
+        case RC_SCAN_DSM:
+            // DSM status output
 #if defined(SPEKTRUM_POWER)
 #endif
-			break;
+            break;
 
-		case RC_SCAN_PPM:
-			// PPM status output
-			break;
+        case RC_SCAN_PPM:
+            // PPM status output
+            break;
 
-		case RC_SCAN_SUMD:
-			// SUMD status output
-			break;
+        case RC_SCAN_SUMD:
+            // SUMD status output
+            break;
 
-		case RC_SCAN_ST24:
-			// SUMD status output
-			break;
-		}
-	}
+        case RC_SCAN_ST24:
+            // SUMD status output
+            break;
+        }
+    }
 
 #if ADC_RC_RSSI_CHANNEL
 
-	if (_analog_rc_rssi_stable) {
-		PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
-	}
+    if (_analog_rc_rssi_stable) {
+        PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f));
+    }
 
 #endif
 
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_publish_interval_perf);
+    perf_print_counter(_cycle_perf);
+    perf_print_counter(_publish_interval_perf);
 
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
-	}
+    if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
+        print_message(ORB_ID(input_rc), _rc_in);
+    }
 
-	return 0;
+    return 0;
 }
 
-int
-RCInput::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
+int RCInput::print_usage(const char *reason) {
+    if (reason) {
+        PX4_WARN("%s\n", reason);
+    }
 
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
+    PRINT_MODULE_DESCRIPTION(
+        R"DESCR_STR(
 ### Description
 This module does the RC input parsing and auto-selecting the method. Supported methods are:
 - PPM
@@ -934,20 +877,19 @@ This module does the RC input parsing and auto-selecting the method. Supported m
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("rc_input", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
+    PRINT_MODULE_USAGE_NAME("rc_input", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
 
 #if defined(SPEKTRUM_POWER)
-	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
+    PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
 #endif /* SPEKTRUM_POWER */
 
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
-	return 0;
+    return 0;
 }
 
-extern "C" __EXPORT int rc_input_main(int argc, char *argv[])
-{
-	return RCInput::main(argc, argv);
+extern "C" __EXPORT int rc_input_main(int argc, char *argv[]) {
+    return RCInput::main(argc, argv);
 }
diff --git a/apps/peripheral/radio_control/rc_input/RCInput.hpp b/apps/peripheral/radio_control/rc_input/RCInput.hpp
index efea2ba231..13a814f086 100644
--- a/apps/peripheral/radio_control/rc_input/RCInput.hpp
+++ b/apps/peripheral/radio_control/rc_input/RCInput.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -64,112 +41,107 @@
 #include "ghst_telemetry.hpp"
 
 #ifdef HRT_PPM_CHANNEL
-# include <systemlib/ppm_decode.h>
+#   include <systemlib/ppm_decode.h>
 #endif
 
-class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::ScheduledWorkItem
-{
+class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::ScheduledWorkItem {
 public:
+    RCInput(const char *device);
+    virtual ~RCInput();
 
-	RCInput(const char *device);
-	virtual ~RCInput();
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
+    /** @see ModuleBase */
+    static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
+    /** @see ModuleBase */
+    static int custom_command(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
+    /** @see ModuleBase */
+    static int print_usage(const char *reason = nullptr);
 
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
+    /** @see ModuleBase::print_status() */
+    int print_status() override;
 
-	int	init();
+    int init();
 
 private:
-
-	enum RC_SCAN {
-		RC_SCAN_NONE = 0,
-		RC_SCAN_PPM  = 1,
-		RC_SCAN_SBUS = 2,
-		RC_SCAN_DSM  = 3,
-		RC_SCAN_ST24 = 5,
-		RC_SCAN_SUMD = 4,
-		RC_SCAN_CRSF = 6,
-		RC_SCAN_GHST = 7,
-	} _rc_scan_state{RC_SCAN_SBUS};
-
-	static constexpr char const *RC_SCAN_STRING[] {
-		"None",
-		"PPM",
-		"SBUS",
-		"DSM",
-		"ST24",
-		"SUMD",
-		"CRSF",
-		"GHST"
-	};
-
-	void Run() override;
+    enum RC_SCAN {
+        RC_SCAN_NONE = 0,
+        RC_SCAN_PPM  = 1,
+        RC_SCAN_SBUS = 2,
+        RC_SCAN_DSM  = 3,
+        RC_SCAN_ST24 = 5,
+        RC_SCAN_SUMD = 4,
+        RC_SCAN_CRSF = 6,
+        RC_SCAN_GHST = 7,
+    } _rc_scan_state{RC_SCAN_SBUS};
+
+    static constexpr char const *RC_SCAN_STRING[]{
+        "None",
+        "PPM",
+        "SBUS",
+        "DSM",
+        "ST24",
+        "SUMD",
+        "CRSF",
+        "GHST"};
+
+    void Run() override;
 
 #if defined(SPEKTRUM_POWER)
-	bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
+    bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const;
 #endif // SPEKTRUM_POWER
 
-	void fill_rc_in(uint16_t raw_rc_count_local,
-			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
-			hrt_abstime now, bool frame_drop, bool failsafe,
-			unsigned frame_drops, int rssi);
+    void fill_rc_in(uint16_t    raw_rc_count_local,
+                    uint16_t    raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
+                    hrt_abstime now, bool frame_drop, bool failsafe,
+                    unsigned frame_drops, int rssi);
 
-	void set_rc_scan_state(RC_SCAN _rc_scan_state);
+    void set_rc_scan_state(RC_SCAN _rc_scan_state);
 
-	void rc_io_invert(bool invert);
+    void rc_io_invert(bool invert);
 
-	hrt_abstime _rc_scan_begin{0};
+    hrt_abstime _rc_scan_begin{0};
 
-	bool _initialized{false};
-	bool _rc_scan_locked{false};
+    bool _initialized{false};
+    bool _rc_scan_locked{false};
 
-	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
+    static constexpr unsigned _current_update_interval{4000}; // 250 Hz
 
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+    uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
-	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+    uORB::Subscription _adc_report_sub{ORB_ID(adc_report)};
+    uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
+    uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	input_rc_s	_rc_in{};
+    input_rc_s _rc_in{};
 
-	float		_analog_rc_rssi_volt{-1.0f};
-	bool		_analog_rc_rssi_stable{false};
+    float _analog_rc_rssi_volt{-1.0f};
+    bool  _analog_rc_rssi_stable{false};
 
-	bool _armed{false};
+    bool _armed{false};
 
 
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+    uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};
 
-	int		_rcs_fd{-1};
-	char		_device[20] {};					///< device / serial port path
+    int  _rcs_fd{-1};
+    char _device[20]{}; ///< device / serial port path
 
-	static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
-	uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
+    static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
+    uint8_t                 _rcs_buf[RC_MAX_BUFFER_SIZE]{};
 
-	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
-	uint16_t _raw_rc_count{};
+    uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]{};
+    uint16_t _raw_rc_count{};
 
-	CRSFTelemetry *_crsf_telemetry{nullptr};
-	GHSTTelemetry *_ghst_telemetry{nullptr};
+    CRSFTelemetry *_crsf_telemetry{nullptr};
+    GHSTTelemetry *_ghst_telemetry{nullptr};
 
-	perf_counter_t	_cycle_perf;
-	perf_counter_t	_publish_interval_perf;
-	uint32_t	_bytes_rx{0};
+    perf_counter_t _cycle_perf;
+    perf_counter_t _publish_interval_perf;
+    uint32_t       _bytes_rx{0};
 
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
-		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
-		(ParamInt<px4::params::RC_INPUT_PROTO>) _param_rc_input_proto
-	)
+    DEFINE_PARAMETERS(
+        (ParamInt<px4::params::RC_RSSI_PWM_CHAN>)_param_rc_rssi_pwm_chan,
+        (ParamInt<px4::params::RC_RSSI_PWM_MIN>)_param_rc_rssi_pwm_min,
+        (ParamInt<px4::params::RC_RSSI_PWM_MAX>)_param_rc_rssi_pwm_max,
+        (ParamInt<px4::params::RC_INPUT_PROTO>)_param_rc_input_proto)
 };
diff --git a/apps/peripheral/radio_control/rc_input/crsf_telemetry.cpp b/apps/peripheral/radio_control/rc_input/crsf_telemetry.cpp
index e11687b5d8..1311000e58 100644
--- a/apps/peripheral/radio_control/rc_input/crsf_telemetry.cpp
+++ b/apps/peripheral/radio_control/rc_input/crsf_telemetry.cpp
@@ -1,183 +1,154 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "crsf_telemetry.h"
 #include <lib/rc/crsf.h>
 
 CRSFTelemetry::CRSFTelemetry(int uart_fd) :
-	_uart_fd(uart_fd)
-{
+    _uart_fd(uart_fd) {
 }
 
-bool CRSFTelemetry::update(const hrt_abstime &now)
-{
-	const int update_rate_hz = 10;
+bool CRSFTelemetry::update(const hrt_abstime &now) {
+    const int update_rate_hz = 10;
 
-	if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
-		return false;
-	}
+    if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
+        return false;
+    }
 
-	bool sent = false;
+    bool sent = false;
 
-	switch (_next_type) {
-	case 0:
-		sent = send_battery();
-		break;
+    switch (_next_type) {
+    case 0:
+        sent = send_battery();
+        break;
 
-	case 1:
-		sent = send_gps();
-		break;
+    case 1:
+        sent = send_gps();
+        break;
 
-	case 2:
-		sent = send_attitude();
-		break;
+    case 2:
+        sent = send_attitude();
+        break;
 
-	case 3:
-		sent = send_flight_mode();
-		break;
-	}
+    case 3:
+        sent = send_flight_mode();
+        break;
+    }
 
-	_last_update = now;
-	_next_type = (_next_type + 1) % num_data_types;
+    _last_update = now;
+    _next_type   = (_next_type + 1) % num_data_types;
 
-	return sent;
+    return sent;
 }
 
-bool CRSFTelemetry::send_battery()
-{
-	battery_status_s battery_status;
+bool CRSFTelemetry::send_battery() {
+    battery_status_s battery_status;
 
-	if (!_battery_status_sub.update(&battery_status)) {
-		return false;
-	}
+    if (!_battery_status_sub.update(&battery_status)) {
+        return false;
+    }
 
-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
-	int fuel = battery_status.discharged_mah;
-	uint8_t remaining = battery_status.remaining * 100;
-	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
+    uint16_t voltage   = battery_status.voltage_filtered_v * 10;
+    uint16_t current   = battery_status.current_filtered_a * 10;
+    int      fuel      = battery_status.discharged_mah;
+    uint8_t  remaining = battery_status.remaining * 100;
+    return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
 }
 
-bool CRSFTelemetry::send_gps()
-{
-	sensor_gps_s vehicle_gps_position;
+bool CRSFTelemetry::send_gps() {
+    sensor_gps_s vehicle_gps_position;
 
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
+    if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+        return false;
+    }
 
-	int32_t latitude = vehicle_gps_position.lat;
-	int32_t longitude = vehicle_gps_position.lon;
-	uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
-	uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
-	uint16_t altitude = vehicle_gps_position.alt + 1000;
-	uint8_t num_satellites = vehicle_gps_position.satellites_used;
+    int32_t  latitude       = vehicle_gps_position.lat;
+    int32_t  longitude      = vehicle_gps_position.lon;
+    uint16_t groundspeed    = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
+    uint16_t gps_heading    = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
+    uint16_t altitude       = vehicle_gps_position.alt + 1000;
+    uint8_t  num_satellites = vehicle_gps_position.satellites_used;
 
-	return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
-				       gps_heading, altitude, num_satellites);
+    return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
+                                   gps_heading, altitude, num_satellites);
 }
 
-bool CRSFTelemetry::send_attitude()
-{
-	vehicle_attitude_s vehicle_attitude;
+bool CRSFTelemetry::send_attitude() {
+    vehicle_attitude_s vehicle_attitude;
 
-	if (!_vehicle_attitude_sub.update(&vehicle_attitude)) {
-		return false;
-	}
+    if (!_vehicle_attitude_sub.update(&vehicle_attitude)) {
+        return false;
+    }
 
-	matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
-	int16_t pitch = attitude(1) * 1e4f;
-	int16_t roll = attitude(0) * 1e4f;
-	int16_t yaw = attitude(2) * 1e4f;
-	return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw);
+    matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
+    int16_t        pitch    = attitude(1) * 1e4f;
+    int16_t        roll     = attitude(0) * 1e4f;
+    int16_t        yaw      = attitude(2) * 1e4f;
+    return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw);
 }
 
-bool CRSFTelemetry::send_flight_mode()
-{
-	vehicle_status_s vehicle_status;
+bool CRSFTelemetry::send_flight_mode() {
+    vehicle_status_s vehicle_status;
 
-	if (!_vehicle_status_sub.update(&vehicle_status)) {
-		return false;
-	}
+    if (!_vehicle_status_sub.update(&vehicle_status)) {
+        return false;
+    }
 
-	const char *flight_mode = "(unknown)";
+    const char *flight_mode = "(unknown)";
 
-	switch (vehicle_status.nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-		flight_mode = "Manual";
-		break;
+    switch (vehicle_status.nav_state) {
+    case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+        flight_mode = "Manual";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
-		flight_mode = "Altitude";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+        flight_mode = "Altitude";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-		flight_mode = "Position";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+        flight_mode = "Position";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-		flight_mode = "Return";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+        flight_mode = "Return";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-		flight_mode = "Mission";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+        flight_mode = "Mission";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
-	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
-		flight_mode = "Auto";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+    case vehicle_status_s::NAVIGATION_STATE_DESCEND:
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
+    case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
+        flight_mode = "Auto";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_ACRO:
-		flight_mode = "Acro";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_ACRO:
+        flight_mode = "Acro";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
-		flight_mode = "Terminate";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
+        flight_mode = "Terminate";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
-		flight_mode = "Offboard";
-		break;
+    case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+        flight_mode = "Offboard";
+        break;
 
-	case vehicle_status_s::NAVIGATION_STATE_STAB:
-		flight_mode = "Stabilized";
-		break;
-	}
+    case vehicle_status_s::NAVIGATION_STATE_STAB:
+        flight_mode = "Stabilized";
+        break;
+    }
 
-	return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode);
+    return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode);
 }
diff --git a/apps/peripheral/radio_control/rc_input/crsf_telemetry.h b/apps/peripheral/radio_control/rc_input/crsf_telemetry.h
index 910a0ec871..9403e7e0c7 100644
--- a/apps/peripheral/radio_control/rc_input/crsf_telemetry.h
+++ b/apps/peripheral/radio_control/rc_input/crsf_telemetry.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *	Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file crsf_telemetry.h
@@ -54,39 +31,38 @@ using namespace time_literals;
 /**
  * High-level class that handles sending of CRSF telemetry data
  */
-class CRSFTelemetry
-{
+class CRSFTelemetry {
 public:
-	/**
+    /**
 	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
 	 * already.
 	 */
-	CRSFTelemetry(int uart_fd);
+    CRSFTelemetry(int uart_fd);
 
-	~CRSFTelemetry() = default;
+    ~CRSFTelemetry() = default;
 
-	/**
+    /**
 	 * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
 	 * limit the sending rate.
 	 * @return true if new data sent
 	 */
-	bool update(const hrt_abstime &now);
+    bool update(const hrt_abstime &now);
 
 private:
-	bool send_battery();
-	bool send_gps();
-	bool send_attitude();
-	bool send_flight_mode();
+    bool send_battery();
+    bool send_gps();
+    bool send_attitude();
+    bool send_flight_mode();
 
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+    uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+    uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+    uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+    uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	hrt_abstime _last_update{0};
+    hrt_abstime _last_update{0};
 
-	static constexpr int num_data_types{4}; ///< number of different telemetry data types
-	int _next_type{0};
+    static constexpr int num_data_types{4}; ///< number of different telemetry data types
+    int                  _next_type{0};
 
-	int _uart_fd;
+    int _uart_fd;
 };
diff --git a/apps/peripheral/radio_control/rc_input/ghst_telemetry.cpp b/apps/peripheral/radio_control/rc_input/ghst_telemetry.cpp
index 2b253bbc3f..0974b22475 100644
--- a/apps/peripheral/radio_control/rc_input/ghst_telemetry.cpp
+++ b/apps/peripheral/radio_control/rc_input/ghst_telemetry.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ghst_telemetry.cpp
@@ -43,97 +20,90 @@
 #include "ghst_telemetry.hpp"
 #include <lib/rc/ghst.hpp>
 
-using time_literals::operator ""_s;
+using time_literals::operator""_s;
 
 GHSTTelemetry::GHSTTelemetry(int uart_fd) :
-	_uart_fd(uart_fd)
-{
+    _uart_fd(uart_fd) {
 }
 
-bool GHSTTelemetry::update(const hrt_abstime &now)
-{
-	bool success = false;
-
-	if ((now - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) {
+bool GHSTTelemetry::update(const hrt_abstime &now) {
+    bool success = false;
 
-		switch (_next_type) {
-		case 0U:
-			success = send_battery_status();
-			break;
+    if ((now - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) {
+        switch (_next_type) {
+        case 0U:
+            success = send_battery_status();
+            break;
 
-		case 1U:
-			success = send_gps1_status();
-			break;
+        case 1U:
+            success = send_gps1_status();
+            break;
 
-		case 2U:
-			success = send_gps2_status();
-			break;
+        case 2U:
+            success = send_gps2_status();
+            break;
 
-		default:
-			success = false;
-			break;
-		}
+        default:
+            success = false;
+            break;
+        }
 
-		_last_update = now;
-		_next_type = (_next_type + 1U) % NUM_DATA_TYPES;
-	}
+        _last_update = now;
+        _next_type   = (_next_type + 1U) % NUM_DATA_TYPES;
+    }
 
-	return success;
+    return success;
 }
 
-bool GHSTTelemetry::send_battery_status()
-{
-	bool success = false;
-	float voltage_in_10mV;
-	float current_in_10mA;
-	float fuel_in_10mAh;
-	battery_status_s battery_status;
-
-	if (_battery_status_sub.update(&battery_status)) {
-		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
-		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
-		success = ghst_send_telemetry_battery_status(_uart_fd,
-				static_cast<uint16_t>(voltage_in_10mV),
-				static_cast<uint16_t>(current_in_10mA),
-				static_cast<uint16_t>(fuel_in_10mAh));
-	}
-
-	return success;
+bool GHSTTelemetry::send_battery_status() {
+    bool             success = false;
+    float            voltage_in_10mV;
+    float            current_in_10mA;
+    float            fuel_in_10mAh;
+    battery_status_s battery_status;
+
+    if (_battery_status_sub.update(&battery_status)) {
+        voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
+        current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+        fuel_in_10mAh   = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
+        success         = ghst_send_telemetry_battery_status(_uart_fd,
+                                                             static_cast<uint16_t>(voltage_in_10mV),
+                                                             static_cast<uint16_t>(current_in_10mA),
+                                                             static_cast<uint16_t>(fuel_in_10mAh));
+    }
+
+    return success;
 }
 
-bool GHSTTelemetry::send_gps1_status()
-{
-	sensor_gps_s vehicle_gps_position;
+bool GHSTTelemetry::send_gps1_status() {
+    sensor_gps_s vehicle_gps_position;
 
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
+    if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+        return false;
+    }
 
-	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
-	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
-	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
+    int32_t  latitude  = vehicle_gps_position.lat;        // 1e-7 degrees
+    int32_t  longitude = vehicle_gps_position.lon;        // 1e-7 degrees
+    uint16_t altitude  = vehicle_gps_position.alt / 1000; // mm -> m
 
-	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
+    return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
 }
 
-bool GHSTTelemetry::send_gps2_status()
-{
-	sensor_gps_s vehicle_gps_position;
+bool GHSTTelemetry::send_gps2_status() {
+    sensor_gps_s vehicle_gps_position;
 
-	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
-		return false;
-	}
+    if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+        return false;
+    }
 
-	uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
-	uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
-	uint8_t num_sats = vehicle_gps_position.satellites_used;
+    uint16_t ground_speed  = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
+    uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
+    uint8_t  num_sats      = vehicle_gps_position.satellites_used;
 
-	// TBD: Can these be computed in a RC telemetry driver?
-	uint16_t home_dist = 0;
-	uint16_t home_dir = 0;
-	uint8_t flags = 0;
+    // TBD: Can these be computed in a RC telemetry driver?
+    uint16_t home_dist = 0;
+    uint16_t home_dir  = 0;
+    uint8_t  flags     = 0;
 
-	return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
+    return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
 }
-
diff --git a/apps/peripheral/radio_control/rc_input/ghst_telemetry.hpp b/apps/peripheral/radio_control/rc_input/ghst_telemetry.hpp
index faa61f69b2..462d93980c 100644
--- a/apps/peripheral/radio_control/rc_input/ghst_telemetry.hpp
+++ b/apps/peripheral/radio_control/rc_input/ghst_telemetry.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ghst_telemetry.cpp
@@ -50,42 +27,40 @@
 /**
  * High-level class that handles sending of GHST telemetry data
  */
-class GHSTTelemetry
-{
+class GHSTTelemetry {
 public:
-	/**
+    /**
 	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
 	 * already.
 	 */
-	explicit GHSTTelemetry(int uart_fd);
+    explicit GHSTTelemetry(int uart_fd);
 
-	~GHSTTelemetry() = default;
+    ~GHSTTelemetry() = default;
 
-	/**
+    /**
 	 * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
 	 * limit the sending rate.
 	 * @return true if new data sent
 	 */
-	bool update(const hrt_abstime &now);
+    bool update(const hrt_abstime &now);
 
 private:
-	bool send_battery_status();
-	bool send_gps1_status();
-	bool send_gps2_status();
-
-	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+    bool send_battery_status();
+    bool send_gps1_status();
+    bool send_gps2_status();
 
-	int _uart_fd;
-	hrt_abstime _last_update {0U};
-	uint32_t _next_type {0U};
+    uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+    uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
-	static constexpr uint32_t NUM_DATA_TYPES {3U};	// number of different telemetry data types
-	static constexpr uint32_t UPDATE_RATE_HZ {10U};	// update rate [Hz]
+    int         _uart_fd;
+    hrt_abstime _last_update{0U};
+    uint32_t    _next_type{0U};
 
-	// Factors that should be applied to get correct values
-	static constexpr float FACTOR_VOLTS_TO_10MV {100.0F};
-	static constexpr float FACTOR_AMPS_TO_10MA {100.0F};
-	static constexpr float FACTOR_MAH_TO_10MAH {0.1F};
+    static constexpr uint32_t NUM_DATA_TYPES{3U};  // number of different telemetry data types
+    static constexpr uint32_t UPDATE_RATE_HZ{10U}; // update rate [Hz]
 
+    // Factors that should be applied to get correct values
+    static constexpr float FACTOR_VOLTS_TO_10MV{100.0F};
+    static constexpr float FACTOR_AMPS_TO_10MA{100.0F};
+    static constexpr float FACTOR_MAH_TO_10MAH{0.1F};
 };
diff --git a/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.cpp b/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.cpp
index 3e321a65a3..bb34e6c0f8 100644
--- a/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.cpp
+++ b/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.cpp
@@ -1,221 +1,190 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "MS4525DO.hpp"
 
 using namespace time_literals;
 
 MS4525DO::MS4525DO(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config)
-{
+    I2C(config),
+    I2CSPIDriver(config) {
 }
 
-MS4525DO::~MS4525DO()
-{
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-	perf_free(_fault_perf);
+MS4525DO::~MS4525DO() {
+    perf_free(_sample_perf);
+    perf_free(_comms_errors);
+    perf_free(_fault_perf);
 }
 
-int MS4525DO::probe()
-{
-	_retries = 1;
-
-	for (int i = 0; i < 10; i++) {
-		// perform 2 x Read_DF3 (Data Fetch 3 Bytes)
-		//  1st read: require status = Normal Operation. Good Data Packet
-		//  2nd read: require status = Stale Data, data should match first read
-		uint8_t data_1[3] {};
-		int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1));
+int MS4525DO::probe() {
+    _retries = 1;
 
-		uint8_t data_2[3] {};
-		int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2));
+    for (int i = 0; i < 10; i++) {
+        // perform 2 x Read_DF3 (Data Fetch 3 Bytes)
+        //  1st read: require status = Normal Operation. Good Data Packet
+        //  2nd read: require status = Stale Data, data should match first read
+        uint8_t data_1[3]{};
+        int     ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1));
 
-		if (ret1 == PX4_OK && ret2 == PX4_OK) {
-			// Status bits
-			const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6;
-			const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6;
+        uint8_t data_2[3]{};
+        int     ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2));
 
-			if ((status_1 == (uint8_t)STATUS::Normal_Operation)
-			    && (status_2 == (uint8_t)STATUS::Stale_Data)
-			    && (data_1[2] == data_1[2])) {
+        if (ret1 == PX4_OK && ret2 == PX4_OK) {
+            // Status bits
+            const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6;
+            const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6;
 
-				_retries = 1; // enable retries during operation
-				return PX4_OK;
+            if ((status_1 == (uint8_t)STATUS::Normal_Operation)
+                && (status_2 == (uint8_t)STATUS::Stale_Data)
+                && (data_1[2] == data_1[2])) {
+                _retries = 1; // enable retries during operation
+                return PX4_OK;
 
-			} else {
-				PX4_ERR("status: %X status: %X", status_1, status_2);
-			}
+            } else {
+                PX4_ERR("status: %X status: %X", status_1, status_2);
+            }
 
-		} else {
-			px4_usleep(1000); // TODO
-		}
-	}
+        } else {
+            px4_usleep(1000); // TODO
+        }
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-int MS4525DO::init()
-{
-	int ret = I2C::init();
+int MS4525DO::init() {
+    int ret = I2C::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("I2C::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("I2C::init failed (%i)", ret);
+        return ret;
+    }
 
-	if (ret == PX4_OK) {
-		ScheduleNow();
-	}
+    if (ret == PX4_OK) {
+        ScheduleNow();
+    }
 
-	return ret;
+    return ret;
 }
 
-void MS4525DO::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void MS4525DO::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	perf_print_counter(_fault_perf);
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
+    perf_print_counter(_fault_perf);
 }
 
-void MS4525DO::RunImpl()
-{
-	switch (_state) {
-	case STATE::MEASURE: {
-			// Send the command to begin a measurement (Read_MR)
-			uint8_t cmd = ADDR_READ_MR;
-
-			if (transfer(&cmd, 1, nullptr, 0) == OK) {
-				_timestamp_sample = hrt_absolute_time();
-				_state = STATE::READ;
-				ScheduleDelayed(2_ms);
-
-			} else {
-				perf_count(_comms_errors);
-				_state = STATE::MEASURE;
-				ScheduleDelayed(10_ms); // try again in 10 ms
-			}
-		}
-
-		break;
-
-	case STATE::READ:
-		// perform 2 x Read_DF4 (Data Fetch 4 Bytes)
-		//  1st read: require status = Normal Operation. Good Data Packet
-		//  2nd read: require status = Stale Data, data should match first read
-		perf_begin(_sample_perf);
-		uint8_t data_1[4] {};
-		int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1));
-
-		uint8_t data_2[4] {};
-		int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2));
-		perf_end(_sample_perf);
-
-		if (ret1 != PX4_OK || ret2 != PX4_OK) {
-			perf_count(_comms_errors);
-
-		} else {
-			// Status bits
-			const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6;
-			const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6;
-
-			const uint8_t bridge_data_1_msb = (data_1[0] & 0b0011'1111);
-			const uint8_t bridge_data_2_msb = (data_2[0] & 0b0011'1111);
-
-			const uint8_t bridge_data_1_lsb = data_1[1];
-			const uint8_t bridge_data_2_lsb = data_2[1];
-
-			// Bridge Data [13:8] + Bridge Data [7:0]
-			int16_t bridge_data_1 = (bridge_data_1_msb << 8) + bridge_data_1_lsb;
-			int16_t bridge_data_2 = (bridge_data_2_msb << 8) + bridge_data_2_lsb;
-
-			// 11-bit temperature data
-			//  Temperature Data [10:3] + Temperature Data [2:0]
-			int16_t temperature_1 = ((data_1[2] << 8) + (0b1110'0000 & data_1[3])) / (1 << 5);
-			int16_t temperature_2 = ((data_2[2] << 8) + (0b1110'0000 & data_2[3])) / (1 << 5);
-
-			if ((status_1 == (uint8_t)STATUS::Fault_Detected) || (status_2 == (uint8_t)STATUS::Fault_Detected)) {
-				// Fault Detected
-				perf_count(_fault_perf);
-
-			} else if ((status_1 == (uint8_t)STATUS::Normal_Operation) && (status_2 == (uint8_t)STATUS::Stale_Data)
-				   && (bridge_data_1_msb == bridge_data_2_msb) && (temperature_1 == temperature_2)) {
-
-				float temperature_c = ((200.f * temperature_1) / 2047) - 50.f;
-
-				// Output is proportional to the difference between Port 1 and Port 2. Output swings
-				// positive when Port 1> Port 2. Output is 50% of supply voltage when Port 1=Port 2.
-
-				// Calculate differential pressure. As its centered around 8000
-				// and can go positive or negative
-				static constexpr float P_min = -1.f; // -1 PSI
-				static constexpr float P_max = 1.f;  // +1 PSI
-
-				// this equation is an inversion of the equation in the
-				// pressure transfer function figure on page 4 of the datasheet
-
-				// We negate the result so that positive differential pressures
-				// are generated when the bottom port is used as the static
-				// port on the pitot and top port is used as the dynamic port
-				const float diff_press_PSI = -((bridge_data_1 - 0.1f * 16383.f) * (P_max - P_min) / (0.8f * 16383.f) + P_min);
-
-				static constexpr float PSI_to_Pa = 6894.757f;
-				float diff_press_pa = diff_press_PSI * PSI_to_Pa;
-
-				if (hrt_elapsed_time(&_timestamp_sample) < 20_ms) {
-					differential_pressure_s differential_pressure{};
-					differential_pressure.timestamp_sample = _timestamp_sample;
-					differential_pressure.device_id = get_device_id();
-					differential_pressure.differential_pressure_pa = diff_press_pa;
-					differential_pressure.temperature = temperature_c;
-					differential_pressure.error_count = perf_event_count(_comms_errors);
-					differential_pressure.timestamp = hrt_absolute_time();
-					_differential_pressure_pub.publish(differential_pressure);
-
-					_timestamp_sample = 0;
-				}
-
-			} else {
-				PX4_DEBUG("status:%X|%X, B:%X|%X, T:%X|%X", status_1, status_2, bridge_data_1, bridge_data_2, temperature_1,
-					  temperature_2);
-			}
-		}
-
-		_state = STATE::MEASURE;
-		ScheduleDelayed(10_ms);
-		break;
-	}
+void MS4525DO::RunImpl() {
+    switch (_state) {
+    case STATE::MEASURE: {
+        // Send the command to begin a measurement (Read_MR)
+        uint8_t cmd = ADDR_READ_MR;
+
+        if (transfer(&cmd, 1, nullptr, 0) == OK) {
+            _timestamp_sample = hrt_absolute_time();
+            _state            = STATE::READ;
+            ScheduleDelayed(2_ms);
+
+        } else {
+            perf_count(_comms_errors);
+            _state = STATE::MEASURE;
+            ScheduleDelayed(10_ms); // try again in 10 ms
+        }
+    }
+
+    break;
+
+    case STATE::READ:
+        // perform 2 x Read_DF4 (Data Fetch 4 Bytes)
+        //  1st read: require status = Normal Operation. Good Data Packet
+        //  2nd read: require status = Stale Data, data should match first read
+        perf_begin(_sample_perf);
+        uint8_t data_1[4]{};
+        int     ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1));
+
+        uint8_t data_2[4]{};
+        int     ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2));
+        perf_end(_sample_perf);
+
+        if (ret1 != PX4_OK || ret2 != PX4_OK) {
+            perf_count(_comms_errors);
+
+        } else {
+            // Status bits
+            const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6;
+            const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6;
+
+            const uint8_t bridge_data_1_msb = (data_1[0] & 0b0011'1111);
+            const uint8_t bridge_data_2_msb = (data_2[0] & 0b0011'1111);
+
+            const uint8_t bridge_data_1_lsb = data_1[1];
+            const uint8_t bridge_data_2_lsb = data_2[1];
+
+            // Bridge Data [13:8] + Bridge Data [7:0]
+            int16_t bridge_data_1 = (bridge_data_1_msb << 8) + bridge_data_1_lsb;
+            int16_t bridge_data_2 = (bridge_data_2_msb << 8) + bridge_data_2_lsb;
+
+            // 11-bit temperature data
+            //  Temperature Data [10:3] + Temperature Data [2:0]
+            int16_t temperature_1 = ((data_1[2] << 8) + (0b1110'0000 & data_1[3])) / (1 << 5);
+            int16_t temperature_2 = ((data_2[2] << 8) + (0b1110'0000 & data_2[3])) / (1 << 5);
+
+            if ((status_1 == (uint8_t)STATUS::Fault_Detected) || (status_2 == (uint8_t)STATUS::Fault_Detected)) {
+                // Fault Detected
+                perf_count(_fault_perf);
+
+            } else if ((status_1 == (uint8_t)STATUS::Normal_Operation) && (status_2 == (uint8_t)STATUS::Stale_Data)
+                       && (bridge_data_1_msb == bridge_data_2_msb) && (temperature_1 == temperature_2)) {
+                float temperature_c = ((200.f * temperature_1) / 2047) - 50.f;
+
+                // Output is proportional to the difference between Port 1 and Port 2. Output swings
+                // positive when Port 1> Port 2. Output is 50% of supply voltage when Port 1=Port 2.
+
+                // Calculate differential pressure. As its centered around 8000
+                // and can go positive or negative
+                static constexpr float P_min = -1.f; // -1 PSI
+                static constexpr float P_max = 1.f;  // +1 PSI
+
+                // this equation is an inversion of the equation in the
+                // pressure transfer function figure on page 4 of the datasheet
+
+                // We negate the result so that positive differential pressures
+                // are generated when the bottom port is used as the static
+                // port on the pitot and top port is used as the dynamic port
+                const float diff_press_PSI = -((bridge_data_1 - 0.1f * 16383.f) * (P_max - P_min) / (0.8f * 16383.f) + P_min);
+
+                static constexpr float PSI_to_Pa     = 6894.757f;
+                float                  diff_press_pa = diff_press_PSI * PSI_to_Pa;
+
+                if (hrt_elapsed_time(&_timestamp_sample) < 20_ms) {
+                    differential_pressure_s differential_pressure{};
+                    differential_pressure.timestamp_sample         = _timestamp_sample;
+                    differential_pressure.device_id                = get_device_id();
+                    differential_pressure.differential_pressure_pa = diff_press_pa;
+                    differential_pressure.temperature              = temperature_c;
+                    differential_pressure.error_count              = perf_event_count(_comms_errors);
+                    differential_pressure.timestamp                = hrt_absolute_time();
+                    _differential_pressure_pub.publish(differential_pressure);
+
+                    _timestamp_sample = 0;
+                }
+
+            } else {
+                PX4_DEBUG("status:%X|%X, B:%X|%X, T:%X|%X", status_1, status_2, bridge_data_1, bridge_data_2, temperature_1,
+                          temperature_2);
+            }
+        }
+
+        _state = STATE::MEASURE;
+        ScheduleDelayed(10_ms);
+        break;
+    }
 }
diff --git a/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.hpp b/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.hpp
index b98483d2f1..6291d30085 100644
--- a/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.hpp
+++ b/apps/peripheral/sensor/airspeed/ms4525do/MS4525DO.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  *
@@ -53,45 +30,44 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/differential_pressure.h>
 
-static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
-static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x28;
+static constexpr uint32_t I2C_SPEED           = 100 * 1000; // 100 kHz I2C serial interface
+static constexpr uint8_t  I2C_ADDRESS_DEFAULT = 0x28;
 
 /* Register address */
 #define ADDR_READ_MR 0x00
 
-class MS4525DO : public device::I2C, public I2CSPIDriver<MS4525DO>
-{
+class MS4525DO : public device::I2C, public I2CSPIDriver<MS4525DO> {
 public:
-	MS4525DO(const I2CSPIDriverConfig &config);
-	~MS4525DO() override;
+    MS4525DO(const I2CSPIDriverConfig &config);
+    ~MS4525DO() override;
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	int probe() override;
+    int probe() override;
 
-	enum class STATE : uint8_t {
-		MEASURE,
-		READ,
-	} _state{STATE::MEASURE};
+    enum class STATE : uint8_t {
+        MEASURE,
+        READ,
+    } _state{STATE::MEASURE};
 
-	enum class STATUS : uint8_t {
-		Normal_Operation = 0b00, // 0: Normal Operation. Good Data Packet
-		Reserved         = 0b01, // 1: Reserved
-		Stale_Data       = 0b10, // 2: Stale Data. Data has been fetched since last measurement cycle.
-		Fault_Detected   = 0b11, // 3: Fault Detected
-	};
+    enum class STATUS : uint8_t {
+        Normal_Operation = 0b00, // 0: Normal Operation. Good Data Packet
+        Reserved         = 0b01, // 1: Reserved
+        Stale_Data       = 0b10, // 2: Stale Data. Data has been fetched since last measurement cycle.
+        Fault_Detected   = 0b11, // 3: Fault Detected
+    };
 
-	hrt_abstime _timestamp_sample{0};
+    hrt_abstime _timestamp_sample{0};
 
-	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
+    uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
 
-	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")};
-	perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": fault detected")};
+    perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": read")};
+    perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME ": communication errors")};
+    perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": fault detected")};
 };
diff --git a/apps/peripheral/sensor/airspeed/ms4525do/ms4525do_main.cpp b/apps/peripheral/sensor/airspeed/ms4525do/ms4525do_main.cpp
index abea2a67f0..e4ab38ac37 100644
--- a/apps/peripheral/sensor/airspeed/ms4525do/ms4525do_main.cpp
+++ b/apps/peripheral/sensor/airspeed/ms4525do/ms4525do_main.cpp
@@ -1,77 +1,52 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "MS4525DO.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void MS4525DO::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("ms4525do", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x28);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void MS4525DO::print_usage() {
+    PRINT_MODULE_USAGE_NAME("ms4525do", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x28);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int ms4525do_main(int argc, char *argv[])
-{
-	using ThisDriver = MS4525DO;
-	BusCLIArguments cli{true, false};
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
-	cli.default_i2c_frequency = I2C_SPEED;
+extern "C" int ms4525do_main(int argc, char *argv[]) {
+    using ThisDriver = MS4525DO;
+    BusCLIArguments cli{true, false};
+    cli.i2c_address           = I2C_ADDRESS_DEFAULT;
+    cli.default_i2c_frequency = I2C_SPEED;
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+    const char *verb = cli.parseDefaultArguments(argc, argv);
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4525DO);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4525DO);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/airspeed/ms4525do/parameters.c b/apps/peripheral/sensor/airspeed/ms4525do/parameters.c
index 86f722ab19..969c38251f 100644
--- a/apps/peripheral/sensor/airspeed/ms4525do/parameters.c
+++ b/apps/peripheral/sensor/airspeed/ms4525do/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * TE MS4525DO differential pressure sensor (external I2C)
diff --git a/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.cpp b/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.cpp
index 4b62025377..0542d476fb 100644
--- a/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.cpp
+++ b/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.cpp
@@ -1,366 +1,332 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "MS5525DSO.hpp"
 
 MS5525DSO::MS5525DSO(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config)
-{
+    I2C(config),
+    I2CSPIDriver(config) {
 }
 
-MS5525DSO::~MS5525DSO()
-{
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
+MS5525DSO::~MS5525DSO() {
+    perf_free(_sample_perf);
+    perf_free(_comms_errors);
 }
 
-int MS5525DSO::probe()
-{
-	_retries = 1;
+int MS5525DSO::probe() {
+    _retries = 1;
 
-	return init_ms5525dso() ? PX4_OK : PX4_ERROR;
+    return init_ms5525dso() ? PX4_OK : PX4_ERROR;
 }
 
-int MS5525DSO::init()
-{
-	int ret = I2C::init();
+int MS5525DSO::init() {
+    int ret = I2C::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("I2C::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("I2C::init failed (%i)", ret);
+        return ret;
+    }
 
-	if (ret == PX4_OK) {
-		ScheduleNow();
-	}
+    if (ret == PX4_OK) {
+        ScheduleNow();
+    }
 
-	return ret;
+    return ret;
 }
 
-void MS5525DSO::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void MS5525DSO::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
 }
 
-int MS5525DSO::measure()
-{
-	int ret = PX4_ERROR;
+int MS5525DSO::measure() {
+    int ret = PX4_ERROR;
 
-	if (_inited) {
-		// send the command to begin a conversion.
-		uint8_t cmd = _current_cmd;
-		ret = transfer(&cmd, 1, nullptr, 0);
+    if (_inited) {
+        // send the command to begin a conversion.
+        uint8_t cmd = _current_cmd;
+        ret         = transfer(&cmd, 1, nullptr, 0);
 
-		if (ret != PX4_OK) {
-			perf_count(_comms_errors);
-		}
+        if (ret != PX4_OK) {
+            perf_count(_comms_errors);
+        }
 
-	} else {
-		_inited = init_ms5525dso();
+    } else {
+        _inited = init_ms5525dso();
 
-		if (_inited) {
-			ret = PX4_OK;
-		}
-	}
+        if (_inited) {
+            ret = PX4_OK;
+        }
+    }
 
-	return ret;
+    return ret;
 }
 
-bool MS5525DSO::init_ms5525dso()
-{
-	// Step 1 - reset
-	uint8_t cmd = CMD_RESET;
-	int ret = transfer(&cmd, 1, nullptr, 0);
+bool MS5525DSO::init_ms5525dso() {
+    // Step 1 - reset
+    uint8_t cmd = CMD_RESET;
+    int     ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (ret != PX4_OK) {
-		perf_count(_comms_errors);
-		return false;
-	}
+    if (ret != PX4_OK) {
+        perf_count(_comms_errors);
+        return false;
+    }
 
-	px4_usleep(3000);
+    px4_usleep(3000);
 
-	// Step 2 - read calibration coefficients from prom
+    // Step 2 - read calibration coefficients from prom
 
-	// prom layout
-	// 0 factory data and the setup
-	// 1-6 calibration coefficients
-	// 7 serial code and CRC
-	uint16_t prom[8] {};
-	bool prom_all_zero = true;
+    // prom layout
+    // 0 factory data and the setup
+    // 1-6 calibration coefficients
+    // 7 serial code and CRC
+    uint16_t prom[8]{};
+    bool     prom_all_zero = true;
 
-	for (uint8_t i = 0; i < 8; i++) {
-		cmd = CMD_PROM_START + i * 2;
+    for (uint8_t i = 0; i < 8; i++) {
+        cmd = CMD_PROM_START + i * 2;
 
-		// request PROM value
-		ret = transfer(&cmd, 1, nullptr, 0);
+        // request PROM value
+        ret = transfer(&cmd, 1, nullptr, 0);
 
-		if (ret != PX4_OK) {
-			perf_count(_comms_errors);
-			return false;
-		}
+        if (ret != PX4_OK) {
+            perf_count(_comms_errors);
+            return false;
+        }
 
-		// read 2 byte value
-		uint8_t val[2];
-		ret = transfer(nullptr, 0, &val[0], 2);
+        // read 2 byte value
+        uint8_t val[2];
+        ret = transfer(nullptr, 0, &val[0], 2);
 
-		if (ret == PX4_OK) {
-			prom[i] = (val[0] << 8) | val[1];
+        if (ret == PX4_OK) {
+            prom[i] = (val[0] << 8) | val[1];
 
-			if (prom[i] != 0) {
-				prom_all_zero = false;
-			}
+            if (prom[i] != 0) {
+                prom_all_zero = false;
+            }
 
-		} else {
-			perf_count(_comms_errors);
-			return false;
-		}
-	}
+        } else {
+            perf_count(_comms_errors);
+            return false;
+        }
+    }
 
-	if (prom_all_zero) {
-		return false;
-	}
+    if (prom_all_zero) {
+        return false;
+    }
 
-	// Step 3 - check CRC
-	const uint8_t crc = prom_crc4(prom);
-	const uint8_t onboard_crc = prom[7] & 0xF;
+    // Step 3 - check CRC
+    const uint8_t crc         = prom_crc4(prom);
+    const uint8_t onboard_crc = prom[7] & 0xF;
 
-	if (crc == onboard_crc) {
-		// store valid calibration coefficients
-		C1 = prom[1];
-		C2 = prom[2];
-		C3 = prom[3];
-		C4 = prom[4];
-		C5 = prom[5];
-		C6 = prom[6];
+    if (crc == onboard_crc) {
+        // store valid calibration coefficients
+        C1 = prom[1];
+        C2 = prom[2];
+        C3 = prom[3];
+        C4 = prom[4];
+        C5 = prom[5];
+        C6 = prom[6];
 
-		Tref = int64_t(C5) * (1UL << Q5);
+        Tref = int64_t(C5) * (1UL << Q5);
 
-		return true;
+        return true;
 
-	} else {
-		PX4_ERR("CRC mismatch");
-	}
+    } else {
+        PX4_ERR("CRC mismatch");
+    }
 
-	return false;
+    return false;
 }
 
-uint8_t MS5525DSO::prom_crc4(uint16_t n_prom[]) const
-{
-	// see Measurement Specialties AN520
+uint8_t MS5525DSO::prom_crc4(uint16_t n_prom[]) const {
+    // see Measurement Specialties AN520
 
-	// crc remainder
-	unsigned int n_rem = 0x00;
+    // crc remainder
+    unsigned int n_rem = 0x00;
 
-	// original value of the crc
-	unsigned int crc_read = n_prom[7]; // save read CRC
-	n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0
+    // original value of the crc
+    unsigned int crc_read = n_prom[7];              // save read CRC
+    n_prom[7]             = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0
 
-	// operation is performed on bytes
-	for (int cnt = 0; cnt < 16; cnt++) {
-		// choose LSB or MSB
-		if (cnt % 2 == 1) {
-			n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF);
+    // operation is performed on bytes
+    for (int cnt = 0; cnt < 16; cnt++) {
+        // choose LSB or MSB
+        if (cnt % 2 == 1) {
+            n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF);
 
-		} else {
-			n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8);
-		}
+        } else {
+            n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8);
+        }
 
-		for (uint8_t n_bit = 8; n_bit > 0; n_bit--) {
-			if (n_rem & (0x8000)) {
-				n_rem = (n_rem << 1) ^ 0x3000;
+        for (uint8_t n_bit = 8; n_bit > 0; n_bit--) {
+            if (n_rem & (0x8000)) {
+                n_rem = (n_rem << 1) ^ 0x3000;
 
-			} else {
-				n_rem = (n_rem << 1);
-			}
-		}
-	}
+            } else {
+                n_rem = (n_rem << 1);
+            }
+        }
+    }
 
-	n_rem = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code
-	n_prom[7] = crc_read; // restore the crc_read to its original place
+    n_rem     = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code
+    n_prom[7] = crc_read;                 // restore the crc_read to its original place
 
-	return (n_rem ^ 0x00);
+    return (n_rem ^ 0x00);
 }
 
-int MS5525DSO::collect()
-{
-	perf_begin(_sample_perf);
-
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
-	// read ADC
-	uint8_t cmd = CMD_ADC_READ;
-	int ret = transfer(&cmd, 1, nullptr, 0);
-
-	if (ret != PX4_OK) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	// read 24 bits from the sensor
-	uint8_t val[3] {};
-	ret = transfer(nullptr, 0, &val[0], 3);
-
-	if (ret != PX4_OK) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2];
-
-	// If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output
-	// result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and
-	// the final result will be wrong. Conversion sequence sent during the already started conversion process will yield
-	// incorrect result as well.
-	if (adc == 0) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return -EAGAIN;
-	}
-
-	if (_current_cmd == CMD_CONVERT_PRES) {
-		D1 = adc;
-		_pressure_count++;
-
-		if (_pressure_count > 9) {
-			_pressure_count = 0;
-			_current_cmd = CMD_CONVERT_TEMP;
-		}
-
-	} else if (_current_cmd == CMD_CONVERT_TEMP) {
-		D2 = adc;
-		_current_cmd = CMD_CONVERT_PRES;
-
-		// only calculate and publish after new pressure readings
-		return PX4_OK;
-	}
-
-	// not ready yet
-	if (D1 == 0 || D2 == 0) {
-		perf_end(_sample_perf);
-		return -EAGAIN;
-	}
-
-	// Difference between actual and reference temperature
-	//  dT = D2 - Tref
-	const int64_t dT = D2 - Tref;
-
-	// Measured temperature
-	//  TEMP = 20°C + dT * TEMPSENS
-	const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6);
-
-	// Offset at actual temperature
-	//  OFF = OFF_T1 + TCO * dT
-	const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4);
-
-	// Sensitivity at actual temperature
-	//  SENS = SENS_T1 + TCS * dT
-	const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3);
-
-	// Temperature Compensated Pressure (example 24996 = 2.4996 psi)
-	//  P = D1 * SENS - OFF
-	const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15);
-
-	const float diff_press_PSI = P * 0.0001f;
-
-	// 1 PSI = 6894.76 Pascals
-	static constexpr float PSI_to_Pa = 6894.757f;
-	const float diff_press_pa = diff_press_PSI * PSI_to_Pa;
-
-	const float temperature_c = TEMP * 0.01f;
-
-	differential_pressure_s differential_pressure{};
-	differential_pressure.timestamp_sample = timestamp_sample;
-	differential_pressure.device_id = get_device_id();
-	differential_pressure.differential_pressure_pa = diff_press_pa;
-	differential_pressure.temperature = temperature_c;
-	differential_pressure.error_count = perf_event_count(_comms_errors);
-	differential_pressure.timestamp = hrt_absolute_time();
-	_differential_pressure_pub.publish(differential_pressure);
-
-	perf_end(_sample_perf);
-
-	return PX4_OK;
+int MS5525DSO::collect() {
+    perf_begin(_sample_perf);
+
+    const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+    // read ADC
+    uint8_t cmd = CMD_ADC_READ;
+    int     ret = transfer(&cmd, 1, nullptr, 0);
+
+    if (ret != PX4_OK) {
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return ret;
+    }
+
+    // read 24 bits from the sensor
+    uint8_t val[3]{};
+    ret = transfer(nullptr, 0, &val[0], 3);
+
+    if (ret != PX4_OK) {
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return ret;
+    }
+
+    uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2];
+
+    // If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output
+    // result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and
+    // the final result will be wrong. Conversion sequence sent during the already started conversion process will yield
+    // incorrect result as well.
+    if (adc == 0) {
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return -EAGAIN;
+    }
+
+    if (_current_cmd == CMD_CONVERT_PRES) {
+        D1 = adc;
+        _pressure_count++;
+
+        if (_pressure_count > 9) {
+            _pressure_count = 0;
+            _current_cmd    = CMD_CONVERT_TEMP;
+        }
+
+    } else if (_current_cmd == CMD_CONVERT_TEMP) {
+        D2           = adc;
+        _current_cmd = CMD_CONVERT_PRES;
+
+        // only calculate and publish after new pressure readings
+        return PX4_OK;
+    }
+
+    // not ready yet
+    if (D1 == 0 || D2 == 0) {
+        perf_end(_sample_perf);
+        return -EAGAIN;
+    }
+
+    // Difference between actual and reference temperature
+    //  dT = D2 - Tref
+    const int64_t dT = D2 - Tref;
+
+    // Measured temperature
+    //  TEMP = 20°C + dT * TEMPSENS
+    const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6);
+
+    // Offset at actual temperature
+    //  OFF = OFF_T1 + TCO * dT
+    const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4);
+
+    // Sensitivity at actual temperature
+    //  SENS = SENS_T1 + TCS * dT
+    const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3);
+
+    // Temperature Compensated Pressure (example 24996 = 2.4996 psi)
+    //  P = D1 * SENS - OFF
+    const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15);
+
+    const float diff_press_PSI = P * 0.0001f;
+
+    // 1 PSI = 6894.76 Pascals
+    static constexpr float PSI_to_Pa     = 6894.757f;
+    const float            diff_press_pa = diff_press_PSI * PSI_to_Pa;
+
+    const float temperature_c = TEMP * 0.01f;
+
+    differential_pressure_s differential_pressure{};
+    differential_pressure.timestamp_sample         = timestamp_sample;
+    differential_pressure.device_id                = get_device_id();
+    differential_pressure.differential_pressure_pa = diff_press_pa;
+    differential_pressure.temperature              = temperature_c;
+    differential_pressure.error_count              = perf_event_count(_comms_errors);
+    differential_pressure.timestamp                = hrt_absolute_time();
+    _differential_pressure_pub.publish(differential_pressure);
+
+    perf_end(_sample_perf);
+
+    return PX4_OK;
 }
 
-void MS5525DSO::RunImpl()
-{
-	int ret = PX4_ERROR;
-
-	// collection phase
-	if (_collect_phase) {
-		// perform collection
-		ret = collect();
+void MS5525DSO::RunImpl() {
+    int ret = PX4_ERROR;
 
-		if (PX4_OK != ret) {
-			perf_count(_comms_errors);
-			/* restart the measurement state machine */
-			_collect_phase = false;
-			_sensor_ok = false;
-			ScheduleNow();
-			return;
-		}
+    // collection phase
+    if (_collect_phase) {
+        // perform collection
+        ret = collect();
 
-		// next phase is measurement
-		_collect_phase = false;
+        if (PX4_OK != ret) {
+            perf_count(_comms_errors);
+            /* restart the measurement state machine */
+            _collect_phase = false;
+            _sensor_ok     = false;
+            ScheduleNow();
+            return;
+        }
 
-		// is there a collect->measure gap?
-		if (_measure_interval > CONVERSION_INTERVAL) {
+        // next phase is measurement
+        _collect_phase = false;
 
-			// schedule a fresh cycle call when we are ready to measure again
-			ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL);
+        // is there a collect->measure gap?
+        if (_measure_interval > CONVERSION_INTERVAL) {
+            // schedule a fresh cycle call when we are ready to measure again
+            ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL);
 
-			return;
-		}
-	}
+            return;
+        }
+    }
 
-	/* measurement phase */
-	ret = measure();
+    /* measurement phase */
+    ret = measure();
 
-	if (PX4_OK != ret) {
-		DEVICE_DEBUG("measure error");
-	}
+    if (PX4_OK != ret) {
+        DEVICE_DEBUG("measure error");
+    }
 
-	_sensor_ok = (ret == OK);
+    _sensor_ok = (ret == OK);
 
-	// next phase is collection
-	_collect_phase = true;
+    // next phase is collection
+    _collect_phase = true;
 
-	// schedule a fresh cycle call when the measurement is done
-	ScheduleDelayed(CONVERSION_INTERVAL);
+    // schedule a fresh cycle call when the measurement is done
+    ScheduleDelayed(CONVERSION_INTERVAL);
 }
diff --git a/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.hpp b/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.hpp
index c6408566aa..4b06045b6d 100644
--- a/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.hpp
+++ b/apps/peripheral/sensor/airspeed/ms5525dso/MS5525DSO.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -43,94 +20,93 @@
 #include <uORB/topics/differential_pressure.h>
 
 /* The MS5525DSODSO address is 111011Cx, where C is the complementary value of the pin CSB */
-static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
-static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x76;
+static constexpr uint32_t I2C_SPEED           = 100 * 1000; // 100 kHz I2C serial interface
+static constexpr uint8_t  I2C_ADDRESS_DEFAULT = 0x76;
 
 /* Measurement rate is 100Hz */
-static constexpr unsigned MEAS_RATE = 100;
-static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
+static constexpr unsigned MEAS_RATE           = 100;
+static constexpr int64_t  CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
 
-class MS5525DSO : public device::I2C, public I2CSPIDriver<MS5525DSO>
-{
+class MS5525DSO : public device::I2C, public I2CSPIDriver<MS5525DSO> {
 public:
-	MS5525DSO(const I2CSPIDriverConfig &config);
-	~MS5525DSO() override;
+    MS5525DSO(const I2CSPIDriverConfig &config);
+    ~MS5525DSO() override;
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	int probe() override;
+    int probe() override;
 
-	bool init_ms5525dso();
+    bool init_ms5525dso();
 
-	uint8_t prom_crc4(uint16_t n_prom[]) const;
+    uint8_t prom_crc4(uint16_t n_prom[]) const;
 
-	int measure();
-	int collect();
+    int measure();
+    int collect();
 
-	static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
-	static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
+    static constexpr uint8_t CMD_RESET    = 0x1E;   // ADC reset command
+    static constexpr uint8_t CMD_ADC_READ = 0x00;   // ADC read command
 
-	static constexpr uint8_t CMD_PROM_START = 0xA0; // Prom read command (first)
+    static constexpr uint8_t CMD_PROM_START = 0xA0; // Prom read command (first)
 
-	// D1 - pressure convert commands
-	// Convert D1 (OSR=256)  0x40
-	// Convert D1 (OSR=512)  0x42
-	// Convert D1 (OSR=1024) 0x44
-	// Convert D1 (OSR=2048) 0x46
-	// Convert D1 (OSR=4096) 0x48
-	static constexpr uint8_t CMD_CONVERT_PRES = 0x48;
+    // D1 - pressure convert commands
+    // Convert D1 (OSR=256)  0x40
+    // Convert D1 (OSR=512)  0x42
+    // Convert D1 (OSR=1024) 0x44
+    // Convert D1 (OSR=2048) 0x46
+    // Convert D1 (OSR=4096) 0x48
+    static constexpr uint8_t CMD_CONVERT_PRES = 0x48;
 
-	// D2 - temperature convert commands
-	// Convert D2 (OSR=256)  0x50
-	// Convert D2 (OSR=512)  0x52
-	// Convert D2 (OSR=1024) 0x54
-	// Convert D2 (OSR=2048) 0x56
-	// Convert D2 (OSR=4096) 0x58
-	static constexpr uint8_t CMD_CONVERT_TEMP = 0x58;
+    // D2 - temperature convert commands
+    // Convert D2 (OSR=256)  0x50
+    // Convert D2 (OSR=512)  0x52
+    // Convert D2 (OSR=1024) 0x54
+    // Convert D2 (OSR=2048) 0x56
+    // Convert D2 (OSR=4096) 0x58
+    static constexpr uint8_t CMD_CONVERT_TEMP = 0x58;
 
-	uint8_t _current_cmd{CMD_CONVERT_PRES};
+    uint8_t _current_cmd{CMD_CONVERT_PRES};
 
-	unsigned _pressure_count{0};
+    unsigned _pressure_count{0};
 
-	// Qx Coefficients Matrix by Pressure Range
-	//  5525DSO-pp001DS (Pmin = -1, Pmax = 1)
-	static constexpr uint8_t Q1 = 15;
-	static constexpr uint8_t Q2 = 17;
-	static constexpr uint8_t Q3 = 7;
-	static constexpr uint8_t Q4 = 5;
-	static constexpr uint8_t Q5 = 7;
-	static constexpr uint8_t Q6 = 21;
+    // Qx Coefficients Matrix by Pressure Range
+    //  5525DSO-pp001DS (Pmin = -1, Pmax = 1)
+    static constexpr uint8_t Q1 = 15;
+    static constexpr uint8_t Q2 = 17;
+    static constexpr uint8_t Q3 = 7;
+    static constexpr uint8_t Q4 = 5;
+    static constexpr uint8_t Q5 = 7;
+    static constexpr uint8_t Q6 = 21;
 
-	// calibration coefficients from prom
-	uint16_t C1{0};
-	uint16_t C2{0};
-	uint16_t C3{0};
-	uint16_t C4{0};
-	uint16_t C5{0};
-	uint16_t C6{0};
+    // calibration coefficients from prom
+    uint16_t C1{0};
+    uint16_t C2{0};
+    uint16_t C3{0};
+    uint16_t C4{0};
+    uint16_t C5{0};
+    uint16_t C6{0};
 
-	int64_t Tref{0};
+    int64_t Tref{0};
 
-	// last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature)
-	uint32_t D1{0};
-	uint32_t D2{0};
+    // last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature)
+    uint32_t D1{0};
+    uint32_t D2{0};
 
-	bool _inited{false};
+    bool _inited{false};
 
-	uint32_t _measure_interval{CONVERSION_INTERVAL};
-	uint32_t _conversion_interval{CONVERSION_INTERVAL};
+    uint32_t _measure_interval{CONVERSION_INTERVAL};
+    uint32_t _conversion_interval{CONVERSION_INTERVAL};
 
-	bool _sensor_ok{false};
-	bool _collect_phase{false};
+    bool _sensor_ok{false};
+    bool _collect_phase{false};
 
-	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
+    uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
 
-	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
-	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")};
+    perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": read")};
+    perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME ": communication errors")};
 };
diff --git a/apps/peripheral/sensor/airspeed/ms5525dso/ms5525dso_main.cpp b/apps/peripheral/sensor/airspeed/ms5525dso/ms5525dso_main.cpp
index a6704f21e9..6cb916bf5f 100644
--- a/apps/peripheral/sensor/airspeed/ms5525dso/ms5525dso_main.cpp
+++ b/apps/peripheral/sensor/airspeed/ms5525dso/ms5525dso_main.cpp
@@ -1,77 +1,52 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2017-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "MS5525DSO.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void MS5525DSO::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("ms5525dso", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void MS5525DSO::print_usage() {
+    PRINT_MODULE_USAGE_NAME("ms5525dso", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int ms5525dso_main(int argc, char *argv[])
-{
-	using ThisDriver = MS5525DSO;
-	BusCLIArguments cli{true, false};
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
-	cli.default_i2c_frequency = I2C_SPEED;
+extern "C" int ms5525dso_main(int argc, char *argv[]) {
+    using ThisDriver = MS5525DSO;
+    BusCLIArguments cli{true, false};
+    cli.i2c_address           = I2C_ADDRESS_DEFAULT;
+    cli.default_i2c_frequency = I2C_SPEED;
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+    const char *verb = cli.parseDefaultArguments(argc, argv);
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS5525DSO);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS5525DSO);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/airspeed/ms5525dso/parameters.c b/apps/peripheral/sensor/airspeed/ms5525dso/parameters.c
index a96d82b981..399b09505d 100644
--- a/apps/peripheral/sensor/airspeed/ms5525dso/parameters.c
+++ b/apps/peripheral/sensor/airspeed/ms5525dso/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * TE MS5525DSO differential pressure sensor (external I2C)
diff --git a/apps/peripheral/sensor/baro/bmp388/bmp388.cpp b/apps/peripheral/sensor/baro/bmp388/bmp388.cpp
index 8178bc5785..aa8eb898ce 100644
--- a/apps/peripheral/sensor/baro/bmp388/bmp388.cpp
+++ b/apps/peripheral/sensor/baro/bmp388/bmp388.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file bmp388.cpp
@@ -42,153 +19,139 @@
 #include "bmp388.h"
 
 BMP388::BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface) :
-	I2CSPIDriver(config),
-	_interface(interface),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
-{
+    I2CSPIDriver(config),
+    _interface(interface),
+    _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": read")),
+    _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": measure")),
+    _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME ": comms errors")) {
 }
 
-BMP388::~BMP388()
-{
-	/* free perf counters */
-	perf_free(_sample_perf);
-	perf_free(_measure_perf);
-	perf_free(_comms_errors);
+BMP388::~BMP388() {
+    /* free perf counters */
+    perf_free(_sample_perf);
+    perf_free(_measure_perf);
+    perf_free(_comms_errors);
 
-	delete _interface;
+    delete _interface;
 }
 
-int
-BMP388::init()
-{
-	if (!soft_reset()) {
-		PX4_DEBUG("failed to reset baro during init");
-		return -EIO;
-	}
+int BMP388::init() {
+    if (!soft_reset()) {
+        PX4_DEBUG("failed to reset baro during init");
+        return -EIO;
+    }
 
-	_chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);
+    _chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);
 
-	if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
-		PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
-		return -EIO;
-	}
+    if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
+        PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
+        return -EIO;
+    }
 
-	if (_chip_id == BMP390_CHIP_ID) {
-		_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
-	}
+    if (_chip_id == BMP390_CHIP_ID) {
+        _interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
+    }
 
-	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
+    _chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
 
-	_cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);
+    _cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);
 
-	if (!_cal) {
-		PX4_WARN("failed to get baro cal init");
-		return -EIO;
-	}
+    if (!_cal) {
+        PX4_WARN("failed to get baro cal init");
+        return -EIO;
+    }
 
-	if (!validate_trimming_param()) {
-		PX4_WARN("failed to validate trim param");
-		return -EIO;
-	}
+    if (!validate_trimming_param()) {
+        PX4_WARN("failed to validate trim param");
+        return -EIO;
+    }
 
-	if (!set_sensor_settings()) {
-		PX4_WARN("failed to set sensor settings");
-		return -EIO;
-	}
+    if (!set_sensor_settings()) {
+        PX4_WARN("failed to set sensor settings");
+        return -EIO;
+    }
 
-	start();
+    start();
 
-	return OK;
+    return OK;
 }
 
-void
-BMP388::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	printf("chip id:  0x%02x rev id:  0x%02x\n", _chip_id, _chip_rev_id);
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_measure_perf);
-	perf_print_counter(_comms_errors);
-	printf("measurement interval:  %u us \n", _measure_interval);
+void BMP388::print_status() {
+    I2CSPIDriverBase::print_status();
+    printf("chip id:  0x%02x rev id:  0x%02x\n", _chip_id, _chip_rev_id);
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_measure_perf);
+    perf_print_counter(_comms_errors);
+    printf("measurement interval:  %u us \n", _measure_interval);
 }
 
-void
-BMP388::start()
-{
-	_collect_phase = false;
+void BMP388::start() {
+    _collect_phase = false;
 
-	// wait a bit longer for the first measurement, as otherwise the first readout might fail
-	ScheduleOnInterval(_measure_interval, _measure_interval * 3);
+    // wait a bit longer for the first measurement, as otherwise the first readout might fail
+    ScheduleOnInterval(_measure_interval, _measure_interval * 3);
 }
 
-void
-BMP388::RunImpl()
-{
-	if (_collect_phase) {
-		collect();
-	}
+void BMP388::RunImpl() {
+    if (_collect_phase) {
+        collect();
+    }
 
-	measure();
+    measure();
 }
 
-int
-BMP388::measure()
-{
-	_collect_phase = true;
+int BMP388::measure() {
+    _collect_phase = true;
 
-	perf_begin(_measure_perf);
+    perf_begin(_measure_perf);
 
-	/* start measurement */
-	if (!set_op_mode(BMP3_FORCED_MODE)) {
-		PX4_DEBUG("failed to set operating mode");
-		perf_count(_comms_errors);
-		perf_cancel(_measure_perf);
-		return -EIO;
-	}
+    /* start measurement */
+    if (!set_op_mode(BMP3_FORCED_MODE)) {
+        PX4_DEBUG("failed to set operating mode");
+        perf_count(_comms_errors);
+        perf_cancel(_measure_perf);
+        return -EIO;
+    }
 
-	perf_end(_measure_perf);
+    perf_end(_measure_perf);
 
-	return OK;
+    return OK;
 }
 
-int
-BMP388::collect()
-{
-	_collect_phase = false;
+int BMP388::collect() {
+    _collect_phase = false;
 
-	/* enable pressure and temperature */
-	uint8_t sensor_comp = BMP3_PRESS | BMP3_TEMP;
-	bmp3_data data{};
+    /* enable pressure and temperature */
+    uint8_t   sensor_comp = BMP3_PRESS | BMP3_TEMP;
+    bmp3_data data{};
 
-	perf_begin(_sample_perf);
+    perf_begin(_sample_perf);
 
-	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+    /* this should be fairly close to the end of the conversion, so the best approximation of the time */
+    const hrt_abstime timestamp_sample = hrt_absolute_time();
 
-	if (!get_sensor_data(sensor_comp, &data)) {
-		perf_count(_comms_errors);
-		perf_cancel(_sample_perf);
-		return -EIO;
-	}
+    if (!get_sensor_data(sensor_comp, &data)) {
+        perf_count(_comms_errors);
+        perf_cancel(_sample_perf);
+        return -EIO;
+    }
 
-	float temperature = (float)(data.temperature / 100.0f);
-	float pressure = (float)(data.pressure / 100.0f); // to Pascal
+    float temperature = (float)(data.temperature / 100.0f);
+    float pressure    = (float)(data.pressure / 100.0f); // to Pascal
 
-	// publish
-	sensor_baro_s sensor_baro{};
-	sensor_baro.timestamp_sample = timestamp_sample;
-	sensor_baro.device_id = _interface->get_device_id();
-	sensor_baro.pressure = pressure;
-	sensor_baro.temperature = temperature;
-	sensor_baro.error_count = perf_event_count(_comms_errors);
-	sensor_baro.timestamp = hrt_absolute_time();
-	_sensor_baro_pub.publish(sensor_baro);
+    // publish
+    sensor_baro_s sensor_baro{};
+    sensor_baro.timestamp_sample = timestamp_sample;
+    sensor_baro.device_id        = _interface->get_device_id();
+    sensor_baro.pressure         = pressure;
+    sensor_baro.temperature      = temperature;
+    sensor_baro.error_count      = perf_event_count(_comms_errors);
+    sensor_baro.timestamp        = hrt_absolute_time();
+    _sensor_baro_pub.publish(sensor_baro);
 
-	perf_end(_sample_perf);
+    perf_end(_sample_perf);
 
-	return OK;
+    return OK;
 }
 
 /*!
@@ -196,29 +159,27 @@ BMP388::collect()
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-bool
-BMP388::soft_reset()
-{
-	bool    result = false;
-	uint8_t status;
-	int     ret;
+bool BMP388::soft_reset() {
+    bool    result = false;
+    uint8_t status;
+    int     ret;
 
-	status = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR);
+    status = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR);
 
-	if (status & BMP3_CMD_RDY) {
-		ret = _interface->set_reg(BPM3_CMD_SOFT_RESET, BMP3_CMD_ADDR);
+    if (status & BMP3_CMD_RDY) {
+        ret = _interface->set_reg(BPM3_CMD_SOFT_RESET, BMP3_CMD_ADDR);
 
-		if (ret == OK) {
-			usleep(BMP3_POST_RESET_WAIT_TIME);
-			status = _interface->get_reg(BMP3_ERR_REG_ADDR);
+        if (ret == OK) {
+            usleep(BMP3_POST_RESET_WAIT_TIME);
+            status = _interface->get_reg(BMP3_ERR_REG_ADDR);
 
-			if ((status & BMP3_CMD_ERR) == 0) {
-				result = true;
-			}
-		}
-	}
+            if ((status & BMP3_CMD_ERR) == 0) {
+                result = true;
+            }
+        }
+    }
 
-	return result;
+    return result;
 }
 
 /*
@@ -226,26 +187,25 @@ BMP388::soft_reset()
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c
  * */
-static int8_t cal_crc(uint8_t seed, uint8_t data)
-{
-	int8_t poly = 0x1D;
-	int8_t var2;
-	uint8_t i;
-
-	for (i = 0; i < 8; i++) {
-		if ((seed & 0x80) ^ (data & 0x80)) {
-			var2 = 1;
-
-		} else {
-			var2 = 0;
-		}
-
-		seed = (seed & 0x7F) << 1;
-		data = (data & 0x7F) << 1;
-		seed = seed ^ (uint8_t)(poly * var2);
-	}
-
-	return (int8_t)seed;
+static int8_t cal_crc(uint8_t seed, uint8_t data) {
+    int8_t  poly = 0x1D;
+    int8_t  var2;
+    uint8_t i;
+
+    for (i = 0; i < 8; i++) {
+        if ((seed & 0x80) ^ (data & 0x80)) {
+            var2 = 1;
+
+        } else {
+            var2 = 0;
+        }
+
+        seed = (seed & 0x7F) << 1;
+        data = (data & 0x7F) << 1;
+        seed = seed ^ (uint8_t)(poly * var2);
+    }
+
+    return (int8_t)seed;
 }
 
 /*
@@ -253,30 +213,27 @@ static int8_t cal_crc(uint8_t seed, uint8_t data)
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c
  * */
-bool
-BMP388::validate_trimming_param()
-{
-	uint8_t crc = 0xFF;
-	uint8_t stored_crc;
-	uint8_t *trim_param = (uint8_t *)_cal;
+bool BMP388::validate_trimming_param() {
+    uint8_t  crc = 0xFF;
+    uint8_t  stored_crc;
+    uint8_t *trim_param = (uint8_t *)_cal;
 
-	static_assert(BMP3_CALIB_DATA_LEN <= sizeof(*_cal), "unexpected struct size");
+    static_assert(BMP3_CALIB_DATA_LEN <= sizeof(*_cal), "unexpected struct size");
 
-	for (int i = 0; i < BMP3_CALIB_DATA_LEN; i++) {
-		crc = (uint8_t)cal_crc(crc, trim_param[i]);
-	}
+    for (int i = 0; i < BMP3_CALIB_DATA_LEN; i++) {
+        crc = (uint8_t)cal_crc(crc, trim_param[i]);
+    }
 
-	crc = (crc ^ 0xFF);
+    crc = (crc ^ 0xFF);
 
-	stored_crc = _interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR);
+    stored_crc = _interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR);
 
-	return stored_crc == crc;
+    return stored_crc == crc;
 }
 
 uint32_t
-BMP388::get_measurement_time()
-{
-	/*
+BMP388::get_measurement_time() {
+    /*
 	  From BST-BMP388-DS001.pdf, page 25, table 21
 
 	  Pressure      Temperature   Measurement     Max Time
@@ -289,40 +246,40 @@ BMP388::get_measurement_time()
 	  x32           2x            68.9 ms         (not documented)
 	*/
 
-	uint32_t meas_time_us = 0; // unsupported value by default
+    uint32_t meas_time_us = 0; // unsupported value by default
 
-	if (osr_t == BMP3_NO_OVERSAMPLING) {
-		switch (osr_p) {
-		case BMP3_NO_OVERSAMPLING:
-			meas_time_us = 5700;
-			break;
+    if (osr_t == BMP3_NO_OVERSAMPLING) {
+        switch (osr_p) {
+        case BMP3_NO_OVERSAMPLING:
+            meas_time_us = 5700;
+            break;
 
-		case BMP3_OVERSAMPLING_2X:
-			meas_time_us = 8700;
-			break;
+        case BMP3_OVERSAMPLING_2X:
+            meas_time_us = 8700;
+            break;
 
-		case BMP3_OVERSAMPLING_4X:
-			meas_time_us = 13300;
-			break;
+        case BMP3_OVERSAMPLING_4X:
+            meas_time_us = 13300;
+            break;
 
-		case BMP3_OVERSAMPLING_8X:
-			meas_time_us = 22500;
-			break;
-		}
+        case BMP3_OVERSAMPLING_8X:
+            meas_time_us = 22500;
+            break;
+        }
 
-	} else if (osr_t == BMP3_OVERSAMPLING_2X) {
-		switch (osr_p) {
-		case BMP3_OVERSAMPLING_16X:
-			meas_time_us = 43300;
-			break;
+    } else if (osr_t == BMP3_OVERSAMPLING_2X) {
+        switch (osr_p) {
+        case BMP3_OVERSAMPLING_16X:
+            meas_time_us = 43300;
+            break;
 
-		case BMP3_OVERSAMPLING_32X:
-			meas_time_us = 68900;
-			break;
-		}
-	}
+        case BMP3_OVERSAMPLING_32X:
+            meas_time_us = 68900;
+            break;
+        }
+    }
 
-	return meas_time_us;
+    return meas_time_us;
 }
 
 /*!
@@ -332,105 +289,100 @@ BMP388::get_measurement_time()
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-bool
-BMP388::set_sensor_settings()
-{
-	_measure_interval = get_measurement_time();
+bool BMP388::set_sensor_settings() {
+    _measure_interval = get_measurement_time();
 
-	if (_measure_interval == 0) {
-		PX4_WARN("unsupported oversampling selected");
-		return false;
-	}
+    if (_measure_interval == 0) {
+        PX4_WARN("unsupported oversampling selected");
+        return false;
+    }
 
-	/* Select the pressure and temperature sensor to be enabled */
-	uint8_t pwc_ctl_reg = 0;
-	pwc_ctl_reg = BMP3_SET_BITS_POS_0(pwc_ctl_reg, BMP3_PRESS_EN, BMP3_ENABLE);
-	pwc_ctl_reg = BMP3_SET_BITS(pwc_ctl_reg, BMP3_TEMP_EN, BMP3_ENABLE);
+    /* Select the pressure and temperature sensor to be enabled */
+    uint8_t pwc_ctl_reg = 0;
+    pwc_ctl_reg         = BMP3_SET_BITS_POS_0(pwc_ctl_reg, BMP3_PRESS_EN, BMP3_ENABLE);
+    pwc_ctl_reg         = BMP3_SET_BITS(pwc_ctl_reg, BMP3_TEMP_EN, BMP3_ENABLE);
 
-	int ret = _interface->set_reg(pwc_ctl_reg, BMP3_PWR_CTRL_ADDR);
+    int ret = _interface->set_reg(pwc_ctl_reg, BMP3_PWR_CTRL_ADDR);
 
-	if (ret != OK) {
-		PX4_WARN("failed to set settings BMP3_PWR_CTRL_ADDR");
-		return false;
-	}
+    if (ret != OK) {
+        PX4_WARN("failed to set settings BMP3_PWR_CTRL_ADDR");
+        return false;
+    }
 
-	/* Select the over sampling settings for pressure and temperature */
-	uint8_t osr_ctl_reg = 0;
-	osr_ctl_reg = BMP3_SET_BITS_POS_0(osr_ctl_reg, BMP3_PRESS_OS, osr_p);
-	osr_ctl_reg = BMP3_SET_BITS(osr_ctl_reg, BMP3_TEMP_OS, osr_t);
+    /* Select the over sampling settings for pressure and temperature */
+    uint8_t osr_ctl_reg = 0;
+    osr_ctl_reg         = BMP3_SET_BITS_POS_0(osr_ctl_reg, BMP3_PRESS_OS, osr_p);
+    osr_ctl_reg         = BMP3_SET_BITS(osr_ctl_reg, BMP3_TEMP_OS, osr_t);
 
-	ret = _interface->set_reg(osr_ctl_reg, BMP3_OSR_ADDR);
+    ret = _interface->set_reg(osr_ctl_reg, BMP3_OSR_ADDR);
 
-	if (ret != OK) {
-		PX4_WARN("failed to set settings BMP3_OSR_ADDR");
-		return false;
-	}
+    if (ret != OK) {
+        PX4_WARN("failed to set settings BMP3_OSR_ADDR");
+        return false;
+    }
 
-	/* Using 'forced mode' so this is not required but here for future use possibly */
-	uint8_t odr_ctl_reg = 0;
-	odr_ctl_reg = BMP3_SET_BITS_POS_0(odr_ctl_reg, BMP3_ODR, odr);
+    /* Using 'forced mode' so this is not required but here for future use possibly */
+    uint8_t odr_ctl_reg = 0;
+    odr_ctl_reg         = BMP3_SET_BITS_POS_0(odr_ctl_reg, BMP3_ODR, odr);
 
-	ret = _interface->set_reg(odr_ctl_reg, BMP3_ODR_ADDR);
+    ret = _interface->set_reg(odr_ctl_reg, BMP3_ODR_ADDR);
 
-	if (ret != OK) {
-		PX4_WARN("failed to set output data rate register");
-		return false;
-	}
+    if (ret != OK) {
+        PX4_WARN("failed to set output data rate register");
+        return false;
+    }
 
-	uint8_t iir_ctl_reg = 0;
-	iir_ctl_reg = BMP3_SET_BITS(iir_ctl_reg, BMP3_IIR_FILTER, iir_coef);
-	ret = _interface->set_reg(iir_ctl_reg, BMP3_IIR_ADDR);
+    uint8_t iir_ctl_reg = 0;
+    iir_ctl_reg         = BMP3_SET_BITS(iir_ctl_reg, BMP3_IIR_FILTER, iir_coef);
+    ret                 = _interface->set_reg(iir_ctl_reg, BMP3_IIR_ADDR);
 
-	if (ret != OK) {
-		PX4_WARN("failed to set IIR settings");
-		return false;
-	}
+    if (ret != OK) {
+        PX4_WARN("failed to set IIR settings");
+        return false;
+    }
 
-	return true;
+    return true;
 }
 
-
 /*!
  * @brief This API sets the power mode of the sensor.
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-bool
-BMP388::set_op_mode(uint8_t op_mode)
-{
-	bool    result = false;
-	uint8_t last_set_mode;
-	uint8_t op_mode_reg_val;
-	int     ret = OK;
-
-	op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
-	last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE);
-
-	/* Device needs to be put in sleep mode to transition */
-	if (last_set_mode != BMP3_SLEEP_MODE) {
-		op_mode_reg_val = op_mode_reg_val & (~(BMP3_OP_MODE_MSK));
-		ret = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR);
-
-		if (ret != OK) {
-			return false;
-		}
-
-		px4_usleep(BMP3_POST_SLEEP_WAIT_TIME);
-	}
-
-	if (ret == OK) {
-		op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
-		op_mode_reg_val = BMP3_SET_BITS(op_mode_reg_val, BMP3_OP_MODE, op_mode);
-		ret = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR);
-
-		if (ret != OK) {
-			return false;
-		}
-
-		result = true;
-	}
-
-	return result;
+bool BMP388::set_op_mode(uint8_t op_mode) {
+    bool    result = false;
+    uint8_t last_set_mode;
+    uint8_t op_mode_reg_val;
+    int     ret = OK;
+
+    op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
+    last_set_mode   = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE);
+
+    /* Device needs to be put in sleep mode to transition */
+    if (last_set_mode != BMP3_SLEEP_MODE) {
+        op_mode_reg_val = op_mode_reg_val & (~(BMP3_OP_MODE_MSK));
+        ret             = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR);
+
+        if (ret != OK) {
+            return false;
+        }
+
+        px4_usleep(BMP3_POST_SLEEP_WAIT_TIME);
+    }
+
+    if (ret == OK) {
+        op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
+        op_mode_reg_val = BMP3_SET_BITS(op_mode_reg_val, BMP3_OP_MODE, op_mode);
+        ret             = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR);
+
+        if (ret != OK) {
+            return false;
+        }
+
+        result = true;
+    }
+
+    return result;
 }
 
 /*!
@@ -439,24 +391,22 @@ BMP388::set_op_mode(uint8_t op_mode)
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data)
-{
-	uint32_t data_xlsb;
-	uint32_t data_lsb;
-	uint32_t data_msb;
-
-	data_xlsb = (uint32_t)reg_data[0];
-	data_lsb = (uint32_t)reg_data[1] << 8;
-	data_msb = (uint32_t)reg_data[2] << 16;
-	uncomp_data->pressure = data_msb | data_lsb | data_xlsb;
-
-	data_xlsb = (uint32_t)reg_data[3];
-	data_lsb = (uint32_t)reg_data[4] << 8;
-	data_msb = (uint32_t)reg_data[5] << 16;
-	uncomp_data->temperature = data_msb | data_lsb | data_xlsb;
+static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data) {
+    uint32_t data_xlsb;
+    uint32_t data_lsb;
+    uint32_t data_msb;
+
+    data_xlsb             = (uint32_t)reg_data[0];
+    data_lsb              = (uint32_t)reg_data[1] << 8;
+    data_msb              = (uint32_t)reg_data[2] << 16;
+    uncomp_data->pressure = data_msb | data_lsb | data_xlsb;
+
+    data_xlsb                = (uint32_t)reg_data[3];
+    data_lsb                 = (uint32_t)reg_data[4] << 8;
+    data_msb                 = (uint32_t)reg_data[5] << 16;
+    uncomp_data->temperature = data_msb | data_lsb | data_xlsb;
 }
 
-
 /*!
  * @brief This internal API is used to compensate the raw temperature data and
  * return the compensated temperature data in integer data type.
@@ -464,28 +414,27 @@ static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data, struct bmp3_calib_data *calib_data)
-{
-	int64_t partial_data1;
-	int64_t partial_data2;
-	int64_t partial_data3;
-	int64_t partial_data4;
-	int64_t partial_data5;
-	int64_t partial_data6;
-	int64_t comp_temp;
-
-	partial_data1 = ((int64_t)uncomp_data->temperature - (256 * calib_data->reg_calib_data.par_t1));
-	partial_data2 = calib_data->reg_calib_data.par_t2 * partial_data1;
-	partial_data3 = (partial_data1 * partial_data1);
-	partial_data4 = (int64_t)partial_data3 * calib_data->reg_calib_data.par_t3;
-	partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
-	partial_data6 = partial_data5 / 4294967296;
-
-	/* Store t_lin in dev. structure for pressure calculation */
-	calib_data->reg_calib_data.t_lin = partial_data6;
-	comp_temp = (int64_t)((partial_data6 * 25) / 16384);
-
-	return comp_temp;
+static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data, struct bmp3_calib_data *calib_data) {
+    int64_t partial_data1;
+    int64_t partial_data2;
+    int64_t partial_data3;
+    int64_t partial_data4;
+    int64_t partial_data5;
+    int64_t partial_data6;
+    int64_t comp_temp;
+
+    partial_data1 = ((int64_t)uncomp_data->temperature - (256 * calib_data->reg_calib_data.par_t1));
+    partial_data2 = calib_data->reg_calib_data.par_t2 * partial_data1;
+    partial_data3 = (partial_data1 * partial_data1);
+    partial_data4 = (int64_t)partial_data3 * calib_data->reg_calib_data.par_t3;
+    partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
+    partial_data6 = partial_data5 / 4294967296;
+
+    /* Store t_lin in dev. structure for pressure calculation */
+    calib_data->reg_calib_data.t_lin = partial_data6;
+    comp_temp                        = (int64_t)((partial_data6 * 25) / 16384);
+
+    return comp_temp;
 }
 
 /*!
@@ -496,44 +445,43 @@ static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
 static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data,
-				    const struct bmp3_calib_data *calib_data)
-{
-	const struct bmp3_reg_calib_data *reg_calib_data = &calib_data->reg_calib_data;
-	int64_t partial_data1;
-	int64_t partial_data2;
-	int64_t partial_data3;
-	int64_t partial_data4;
-	int64_t partial_data5;
-	int64_t partial_data6;
-	int64_t offset;
-	int64_t sensitivity;
-	uint64_t comp_press;
-
-	partial_data1 = reg_calib_data->t_lin * reg_calib_data->t_lin;
-	partial_data2 = partial_data1 / 64;
-	partial_data3 = (partial_data2 * reg_calib_data->t_lin) / 256;
-	partial_data4 = (reg_calib_data->par_p8 * partial_data3) / 32;
-	partial_data5 = (reg_calib_data->par_p7 * partial_data1) * 16;
-	partial_data6 = (reg_calib_data->par_p6 * reg_calib_data->t_lin) * 4194304;
-	offset = (reg_calib_data->par_p5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
-	partial_data2 = (reg_calib_data->par_p4 * partial_data3) / 32;
-	partial_data4 = (reg_calib_data->par_p3 * partial_data1) * 4;
-	partial_data5 = (reg_calib_data->par_p2 - 16384) * reg_calib_data->t_lin * 2097152;
-	sensitivity = ((reg_calib_data->par_p1 - 16384) * 70368744177664) + partial_data2 + partial_data4 + partial_data5;
-	partial_data1 = (sensitivity / 16777216) * uncomp_data->pressure;
-	partial_data2 = reg_calib_data->par_p10 * reg_calib_data->t_lin;
-	partial_data3 = partial_data2 + (65536 * reg_calib_data->par_p9);
-	partial_data4 = (partial_data3 * uncomp_data->pressure) / 8192;
-	/*dividing by 10 followed by multiplying by 10 to avoid overflow caused by (uncomp_data->pressure * partial_data4) */
-	partial_data5 = (uncomp_data->pressure * (partial_data4 / 10)) / 512;
-	partial_data5 = partial_data5 * 10;
-	partial_data6 = (int64_t)((uint64_t)uncomp_data->pressure * (uint64_t)uncomp_data->pressure);
-	partial_data2 = (reg_calib_data->par_p11 * partial_data6) / 65536;
-	partial_data3 = (partial_data2 * uncomp_data->pressure) / 128;
-	partial_data4 = (offset / 4) + partial_data1 + partial_data5 + partial_data3;
-	comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
-
-	return comp_press;
+                                    const struct bmp3_calib_data  *calib_data) {
+    const struct bmp3_reg_calib_data *reg_calib_data = &calib_data->reg_calib_data;
+    int64_t                           partial_data1;
+    int64_t                           partial_data2;
+    int64_t                           partial_data3;
+    int64_t                           partial_data4;
+    int64_t                           partial_data5;
+    int64_t                           partial_data6;
+    int64_t                           offset;
+    int64_t                           sensitivity;
+    uint64_t                          comp_press;
+
+    partial_data1 = reg_calib_data->t_lin * reg_calib_data->t_lin;
+    partial_data2 = partial_data1 / 64;
+    partial_data3 = (partial_data2 * reg_calib_data->t_lin) / 256;
+    partial_data4 = (reg_calib_data->par_p8 * partial_data3) / 32;
+    partial_data5 = (reg_calib_data->par_p7 * partial_data1) * 16;
+    partial_data6 = (reg_calib_data->par_p6 * reg_calib_data->t_lin) * 4194304;
+    offset        = (reg_calib_data->par_p5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
+    partial_data2 = (reg_calib_data->par_p4 * partial_data3) / 32;
+    partial_data4 = (reg_calib_data->par_p3 * partial_data1) * 4;
+    partial_data5 = (reg_calib_data->par_p2 - 16384) * reg_calib_data->t_lin * 2097152;
+    sensitivity   = ((reg_calib_data->par_p1 - 16384) * 70368744177664) + partial_data2 + partial_data4 + partial_data5;
+    partial_data1 = (sensitivity / 16777216) * uncomp_data->pressure;
+    partial_data2 = reg_calib_data->par_p10 * reg_calib_data->t_lin;
+    partial_data3 = partial_data2 + (65536 * reg_calib_data->par_p9);
+    partial_data4 = (partial_data3 * uncomp_data->pressure) / 8192;
+    /*dividing by 10 followed by multiplying by 10 to avoid overflow caused by (uncomp_data->pressure * partial_data4) */
+    partial_data5 = (uncomp_data->pressure * (partial_data4 / 10)) / 512;
+    partial_data5 = partial_data5 * 10;
+    partial_data6 = (int64_t)((uint64_t)uncomp_data->pressure * (uint64_t)uncomp_data->pressure);
+    partial_data2 = (reg_calib_data->par_p11 * partial_data6) / 65536;
+    partial_data3 = (partial_data2 * uncomp_data->pressure) / 128;
+    partial_data4 = (offset / 4) + partial_data1 + partial_data5 + partial_data3;
+    comp_press    = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
+
+    return comp_press;
 }
 
 /*!
@@ -542,30 +490,28 @@ static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data,
  *
  * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
  */
-bool
-BMP388::compensate_data(uint8_t sensor_comp,
-			const struct bmp3_uncomp_data *uncomp_data,
-			struct bmp3_data *comp_data)
-{
-	int8_t rslt = OK;
-	struct bmp3_calib_data calib_data = {0};
-	struct bmp3_reg_calib_data *reg_calib_data = &calib_data.reg_calib_data;
-	memcpy(reg_calib_data, _cal, 21);
-
-	if ((uncomp_data != NULL) && (comp_data != NULL)) {
-		if (sensor_comp & (BMP3_PRESS | BMP3_TEMP)) {
-			comp_data->temperature = compensate_temperature(uncomp_data, &calib_data);
-		}
-
-		if (sensor_comp & BMP3_PRESS) {
-			comp_data->pressure = compensate_pressure(uncomp_data, &calib_data);
-		}
-
-	} else {
-		rslt = -1;
-	}
-
-	return (rslt == 0);
+bool BMP388::compensate_data(uint8_t                        sensor_comp,
+                             const struct bmp3_uncomp_data *uncomp_data,
+                             struct bmp3_data              *comp_data) {
+    int8_t                      rslt           = OK;
+    struct bmp3_calib_data      calib_data     = {0};
+    struct bmp3_reg_calib_data *reg_calib_data = &calib_data.reg_calib_data;
+    memcpy(reg_calib_data, _cal, 21);
+
+    if ((uncomp_data != NULL) && (comp_data != NULL)) {
+        if (sensor_comp & (BMP3_PRESS | BMP3_TEMP)) {
+            comp_data->temperature = compensate_temperature(uncomp_data, &calib_data);
+        }
+
+        if (sensor_comp & BMP3_PRESS) {
+            comp_data->pressure = compensate_pressure(uncomp_data, &calib_data);
+        }
+
+    } else {
+        rslt = -1;
+    }
+
+    return (rslt == 0);
 }
 
 /*!
@@ -573,28 +519,26 @@ BMP388::compensate_data(uint8_t sensor_comp,
  * sensor, compensates the data and store it in the bmp3_data structure
  * instance passed by the user.
  */
-bool
-BMP388::get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data)
-{
-	bool result = false;
-	int8_t rslt;
+bool BMP388::get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data) {
+    bool   result = false;
+    int8_t rslt;
 
-	uint8_t reg_data[BMP3_P_T_DATA_LEN];
-	struct bmp3_uncomp_data uncomp_data;
+    uint8_t                 reg_data[BMP3_P_T_DATA_LEN];
+    struct bmp3_uncomp_data uncomp_data;
 
-	rslt = _interface->get_reg_buf(BMP3_SENS_STATUS_REG_ADDR, reg_data, BMP3_P_T_DATA_LEN);
+    rslt = _interface->get_reg_buf(BMP3_SENS_STATUS_REG_ADDR, reg_data, BMP3_P_T_DATA_LEN);
 
-	if (rslt == OK) {
-		uint8_t status = reg_data[0];
+    if (rslt == OK) {
+        uint8_t status = reg_data[0];
 
-		// check if data ready (both temp and pressure)
-		if ((status & (3 << 5)) != (3 << 5)) {
-			return false;
-		}
+        // check if data ready (both temp and pressure)
+        if ((status & (3 << 5)) != (3 << 5)) {
+            return false;
+        }
 
-		parse_sensor_data(reg_data + 1, &uncomp_data);
-		result = compensate_data(sensor_comp, &uncomp_data, comp_data);
-	}
+        parse_sensor_data(reg_data + 1, &uncomp_data);
+        result = compensate_data(sensor_comp, &uncomp_data, comp_data);
+    }
 
-	return result;
+    return result;
 }
diff --git a/apps/peripheral/sensor/baro/bmp388/bmp388.h b/apps/peripheral/sensor/baro/bmp388/bmp388.h
index 2db3e22382..8c43885221 100644
--- a/apps/peripheral/sensor/baro/bmp388/bmp388.h
+++ b/apps/peripheral/sensor/baro/bmp388/bmp388.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file bmp388.h
@@ -51,206 +28,205 @@
 
 // From https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h
 
-#define BMP388_CHIP_ID                    (0x50)
-#define BMP390_CHIP_ID                    (0x60)
+#define BMP388_CHIP_ID (0x50)
+#define BMP390_CHIP_ID (0x60)
 
 /* Over sampling macros */
-#define BMP3_NO_OVERSAMPLING              (0x00)
-#define BMP3_OVERSAMPLING_2X              (0x01)
-#define BMP3_OVERSAMPLING_4X              (0x02)
-#define BMP3_OVERSAMPLING_8X              (0x03)
-#define BMP3_OVERSAMPLING_16X             (0x04)
-#define BMP3_OVERSAMPLING_32X             (0x05)
+#define BMP3_NO_OVERSAMPLING  (0x00)
+#define BMP3_OVERSAMPLING_2X  (0x01)
+#define BMP3_OVERSAMPLING_4X  (0x02)
+#define BMP3_OVERSAMPLING_8X  (0x03)
+#define BMP3_OVERSAMPLING_16X (0x04)
+#define BMP3_OVERSAMPLING_32X (0x05)
 
 /* Filter setting macros */
-#define BMP3_IIR_FILTER_DISABLE           (0x00)
-#define BMP3_IIR_FILTER_COEFF_1           (0x01)
-#define BMP3_IIR_FILTER_COEFF_3           (0x02)
-#define BMP3_IIR_FILTER_COEFF_7           (0x03)
-#define BMP3_IIR_FILTER_COEFF_15          (0x04)
-#define BMP3_IIR_FILTER_COEFF_31          (0x05)
-#define BMP3_IIR_FILTER_COEFF_63          (0x06)
-#define BMP3_IIR_FILTER_COEFF_127         (0x07)
+#define BMP3_IIR_FILTER_DISABLE   (0x00)
+#define BMP3_IIR_FILTER_COEFF_1   (0x01)
+#define BMP3_IIR_FILTER_COEFF_3   (0x02)
+#define BMP3_IIR_FILTER_COEFF_7   (0x03)
+#define BMP3_IIR_FILTER_COEFF_15  (0x04)
+#define BMP3_IIR_FILTER_COEFF_31  (0x05)
+#define BMP3_IIR_FILTER_COEFF_63  (0x06)
+#define BMP3_IIR_FILTER_COEFF_127 (0x07)
 
 /* Odr setting macros */
-#define BMP3_ODR_200_HZ                   (0x00)
-#define BMP3_ODR_100_HZ                   (0x01)
-#define BMP3_ODR_50_HZ                    (0x02)
-#define BMP3_ODR_25_HZ                    (0x03)
+#define BMP3_ODR_200_HZ (0x00)
+#define BMP3_ODR_100_HZ (0x01)
+#define BMP3_ODR_50_HZ  (0x02)
+#define BMP3_ODR_25_HZ  (0x03)
 
 /* Register Address */
-#define BMP3_CHIP_ID_ADDR                 (0x00)
-#define BMP3_REV_ID_ADDR                  (0x01)
-#define BMP3_ERR_REG_ADDR                 (0x02)
-#define BMP3_SENS_STATUS_REG_ADDR         (0x03)
-#define BMP3_DATA_ADDR                    (0x04)
-#define BMP3_PWR_CTRL_ADDR                (0x1B)
-#define BMP3_OSR_ADDR                     (0X1C)
-#define BMP3_CALIB_DATA_ADDR              (0x31)
-#define BMP3_CMD_ADDR                     (0x7E)
+#define BMP3_CHIP_ID_ADDR         (0x00)
+#define BMP3_REV_ID_ADDR          (0x01)
+#define BMP3_ERR_REG_ADDR         (0x02)
+#define BMP3_SENS_STATUS_REG_ADDR (0x03)
+#define BMP3_DATA_ADDR            (0x04)
+#define BMP3_PWR_CTRL_ADDR        (0x1B)
+#define BMP3_OSR_ADDR             (0X1C)
+#define BMP3_CALIB_DATA_ADDR      (0x31)
+#define BMP3_CMD_ADDR             (0x7E)
 
 /* Error status macros */
-#define BMP3_FATAL_ERR                    (0x01)
-#define BMP3_CMD_ERR                      (0x02)
-#define BMP3_CONF_ERR                     (0x04)
+#define BMP3_FATAL_ERR (0x01)
+#define BMP3_CMD_ERR   (0x02)
+#define BMP3_CONF_ERR  (0x04)
 
 /* Status macros */
-#define BMP3_CMD_RDY                      (0x10)
-#define BMP3_DRDY_PRESS                   (0x20)
-#define BMP3_DRDY_TEMP                    (0x40)
+#define BMP3_CMD_RDY    (0x10)
+#define BMP3_DRDY_PRESS (0x20)
+#define BMP3_DRDY_TEMP  (0x40)
 
 /* Power mode macros */
-#define BMP3_SLEEP_MODE                   (0x00)
-#define BMP3_FORCED_MODE                  (0x01)
-#define BMP3_NORMAL_MODE                  (0x03)
+#define BMP3_SLEEP_MODE  (0x00)
+#define BMP3_FORCED_MODE (0x01)
+#define BMP3_NORMAL_MODE (0x03)
 
-#define BMP3_ENABLE                       (0x01)
-#define BMP3_DISABLE                      (0x00)
+#define BMP3_ENABLE      (0x01)
+#define BMP3_DISABLE     (0x00)
 
 /* Sensor component selection macros.  These values are internal for API implementation.
  * Don't relate this t0 data sheet.
  */
-#define BMP3_PRESS                        (1)
-#define BMP3_TEMP                         (1 << 1)
-#define BMP3_ALL                          (0x03)
+#define BMP3_PRESS (1)
+#define BMP3_TEMP  (1 << 1)
+#define BMP3_ALL   (0x03)
 
 /* Macros related to size */
-#define BMP3_CALIB_DATA_LEN               (21)
-#define BMP3_P_T_DATA_LEN                 (7)
+#define BMP3_CALIB_DATA_LEN (21)
+#define BMP3_P_T_DATA_LEN   (7)
 
 /* Macros to select the which sensor settings are to be set by the user.
  * These values are internal for API implementation. Don't relate this to
  * data sheet. */
-#define BMP3_PRESS_EN_SEL                 (1 << 1)
-#define BMP3_TEMP_EN_SEL                  (1 << 2)
-#define BMP3_PRESS_OS_SEL                 (1 << 4)
+#define BMP3_PRESS_EN_SEL (1 << 1)
+#define BMP3_TEMP_EN_SEL  (1 << 2)
+#define BMP3_PRESS_OS_SEL (1 << 4)
 
 /* Macros for bit masking */
-#define BMP3_OP_MODE_MSK                  (0x30)
-#define BMP3_OP_MODE_POS                  (0x04)
+#define BMP3_OP_MODE_MSK    (0x30)
+#define BMP3_OP_MODE_POS    (0x04)
 
-#define BMP3_PRESS_EN_MSK                 (0x01)
+#define BMP3_PRESS_EN_MSK   (0x01)
 
-#define BMP3_TEMP_EN_MSK                  (0x02)
-#define BMP3_TEMP_EN_POS                  (0x01)
+#define BMP3_TEMP_EN_MSK    (0x02)
+#define BMP3_TEMP_EN_POS    (0x01)
 
-#define BMP3_IIR_FILTER_MSK               (0x0E)
-#define BMP3_IIR_FILTER_POS               (0x01)
+#define BMP3_IIR_FILTER_MSK (0x0E)
+#define BMP3_IIR_FILTER_POS (0x01)
 
-#define BMP3_ODR_MSK                      (0x1F)
+#define BMP3_ODR_MSK        (0x1F)
 
-#define BMP3_PRESS_OS_MSK                 (0x07)
+#define BMP3_PRESS_OS_MSK   (0x07)
 
-#define BMP3_TEMP_OS_MSK                  (0x38)
-#define BMP3_TEMP_OS_POS                  (0x03)
+#define BMP3_TEMP_OS_MSK    (0x38)
+#define BMP3_TEMP_OS_POS    (0x03)
 
 #define BMP3_SET_BITS(reg_data, bitname, data) \
-	((reg_data & ~(bitname##_MSK)) | \
-	 ((data << bitname##_POS) & bitname##_MSK))
+    ((reg_data & ~(bitname##_MSK)) | ((data << bitname##_POS) & bitname##_MSK))
 
 /* Macro variant to handle the bitname position if it is zero */
 #define BMP3_SET_BITS_POS_0(reg_data, bitname, data) \
-	((reg_data & ~(bitname##_MSK)) | \
-	 (data & bitname##_MSK))
+    ((reg_data & ~(bitname##_MSK)) | (data & bitname##_MSK))
 
-#define BMP3_GET_BITS(reg_data, bitname)       ((reg_data & (bitname##_MSK)) >> \
-		(bitname##_POS))
+#define BMP3_GET_BITS(reg_data, bitname) ((reg_data & (bitname##_MSK)) >> (bitname##_POS))
 
 /* Macro variant to handle the bitname position if it is zero */
 #define BMP3_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK))
 
 // From https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c
-#define BMP3_POST_SLEEP_WAIT_TIME         5000
-#define BMP3_POST_RESET_WAIT_TIME         2000
-#define BMP3_POST_INIT_WAIT_TIME          40000
-#define BMP3_TRIM_CRC_DATA_ADDR           0x30
-#define BPM3_CMD_SOFT_RESET               0xB6
-#define BMP3_ODR_ADDR                     0x1D
-#define BMP3_IIR_ADDR                     0x1F
+#define BMP3_POST_SLEEP_WAIT_TIME 5000
+#define BMP3_POST_RESET_WAIT_TIME 2000
+#define BMP3_POST_INIT_WAIT_TIME  40000
+#define BMP3_TRIM_CRC_DATA_ADDR   0x30
+#define BPM3_CMD_SOFT_RESET       0xB6
+#define BMP3_ODR_ADDR             0x1D
+#define BMP3_IIR_ADDR             0x1F
 
 // https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
 /* Power control settings */
-#define POWER_CNTL            (0x0006)
+#define POWER_CNTL (0x0006)
 /* Odr and filter settings */
-#define ODR_FILTER            (0x00F0)
+#define ODR_FILTER (0x00F0)
 /* Interrupt control settings */
-#define INT_CTRL              (0x0708)
+#define INT_CTRL (0x0708)
 /* Advance settings */
-#define ADV_SETT              (0x1800)
+#define ADV_SETT (0x1800)
+
+#pragma pack(push, 1)
 
-#pragma pack(push,1)
 struct calibration_s {
-	uint16_t par_t1;
-	uint16_t par_t2;
-	int8_t   par_t3;
-
-	int16_t  par_p1;
-	int16_t  par_p2;
-	int8_t   par_p3;
-	int8_t   par_p4;
-	uint16_t par_p5;
-	uint16_t par_p6;
-	int8_t   par_p7;
-	int8_t   par_p8;
-	int16_t  par_p9;
-	int8_t   par_p10;
-	int8_t   par_p11;
+    uint16_t par_t1;
+    uint16_t par_t2;
+    int8_t   par_t3;
+
+    int16_t  par_p1;
+    int16_t  par_p2;
+    int8_t   par_p3;
+    int8_t   par_p4;
+    uint16_t par_p5;
+    uint16_t par_p6;
+    int8_t   par_p7;
+    int8_t   par_p8;
+    int16_t  par_p9;
+    int8_t   par_p10;
+    int8_t   par_p11;
 
 }; //calibration data
 
 struct data_s {
-	uint8_t p_msb;
-	uint8_t p_lsb;
-	uint8_t p_xlsb;
+    uint8_t p_msb;
+    uint8_t p_lsb;
+    uint8_t p_xlsb;
 
-	uint8_t t_msb;
-	uint8_t t_lsb;
-	uint8_t t_xlsb;
+    uint8_t t_msb;
+    uint8_t t_lsb;
+    uint8_t t_xlsb;
 }; // data
 
 struct bmp3_reg_calib_data {
-	/**
+    /**
 	 * @ Trim Variables
 	 */
 
-	/**@{*/
-	uint16_t par_t1;
-	uint16_t par_t2;
-	int8_t par_t3;
-	int16_t par_p1;
-	int16_t par_p2;
-	int8_t par_p3;
-	int8_t par_p4;
-	uint16_t par_p5;
-	uint16_t par_p6;
-	int8_t par_p7;
-	int8_t par_p8;
-	int16_t par_p9;
-	int8_t par_p10;
-	int8_t par_p11;
-	int64_t t_lin;
-
-	/**@}*/
+    /**@{*/
+    uint16_t par_t1;
+    uint16_t par_t2;
+    int8_t   par_t3;
+    int16_t  par_p1;
+    int16_t  par_p2;
+    int8_t   par_p3;
+    int8_t   par_p4;
+    uint16_t par_p5;
+    uint16_t par_p6;
+    int8_t   par_p7;
+    int8_t   par_p8;
+    int16_t  par_p9;
+    int8_t   par_p10;
+    int8_t   par_p11;
+    int64_t  t_lin;
+
+    /**@}*/
 };
+
 #pragma pack(pop)
 
 /*!
  * bmp3 sensor structure which comprises of temperature and pressure data.
  */
 struct bmp3_data {
-	/* Compensated temperature */
-	int64_t temperature;
+    /* Compensated temperature */
+    int64_t temperature;
 
-	/* Compensated pressure */
-	uint64_t pressure;
+    /* Compensated pressure */
+    uint64_t pressure;
 };
 
 /*!
  * Calibration data
  */
 struct bmp3_calib_data {
-	/*! Register data */
-	struct bmp3_reg_calib_data reg_calib_data;
+    /*! Register data */
+    struct bmp3_reg_calib_data reg_calib_data;
 };
 
 /*!
@@ -258,110 +234,108 @@ struct bmp3_calib_data {
  * and pressure data.
  */
 struct bmp3_uncomp_data {
-	/*! un-compensated pressure */
-	uint32_t pressure;
+    /*! un-compensated pressure */
+    uint32_t pressure;
 
-	/*! un-compensated temperature */
-	uint32_t temperature;
+    /*! un-compensated temperature */
+    uint32_t temperature;
 };
 
 struct fcalibration_s {
-	float t1;
-	float t2;
-	float t3;
-
-	float p1;
-	float p2;
-	float p3;
-	float p4;
-	float p5;
-	float p6;
-	float p7;
-	float p8;
-	float p9;
+    float t1;
+    float t2;
+    float t3;
+
+    float p1;
+    float p2;
+    float p3;
+    float p4;
+    float p5;
+    float p6;
+    float p7;
+    float p8;
+    float p9;
 };
 
 /*
  * BMP388 internal constants and data structures.
  */
-class IBMP388
-{
+class IBMP388 {
 public:
-	virtual ~IBMP388() = default;
+    virtual ~IBMP388() = default;
 
-	virtual int init() = 0;
+    virtual int init() = 0;
 
-	// read reg value
-	virtual uint8_t get_reg(uint8_t addr) = 0;
+    // read reg value
+    virtual uint8_t get_reg(uint8_t addr) = 0;
 
-	// bulk read reg value
-	virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
+    // bulk read reg value
+    virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
 
-	// write reg value
-	virtual int set_reg(uint8_t value, uint8_t addr) = 0;
+    // write reg value
+    virtual int set_reg(uint8_t value, uint8_t addr) = 0;
 
-	// bulk read of calibration data into buffer, return same pointer
-	virtual calibration_s *get_calibration(uint8_t addr) = 0;
+    // bulk read of calibration data into buffer, return same pointer
+    virtual calibration_s *get_calibration(uint8_t addr) = 0;
 
-	virtual uint32_t get_device_id() const = 0;
+    virtual uint32_t get_device_id() const = 0;
 
-	virtual uint8_t get_device_address() const = 0;
+    virtual uint8_t get_device_address() const = 0;
 
-	virtual void set_device_type(uint8_t devtype) = 0;
+    virtual void set_device_type(uint8_t devtype) = 0;
 };
 
-class BMP388 : public I2CSPIDriver<BMP388>
-{
+class BMP388 : public I2CSPIDriver<BMP388> {
 public:
-	BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface);
-	virtual ~BMP388();
+    BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface);
+    virtual ~BMP388();
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	virtual int		init();
+    virtual int init();
 
-	void			print_status();
+    void print_status();
+
+    void RunImpl();
 
-	void 			RunImpl();
 private:
-	static constexpr uint8_t			osr_t{BMP3_OVERSAMPLING_2X};		// oversampling rate, temperature
-	static constexpr uint8_t			osr_p{BMP3_OVERSAMPLING_16X};		// oversampling rate, pressure
-	static constexpr uint8_t			odr{BMP3_ODR_50_HZ};			// output data rate (not used)
-	static constexpr uint8_t			iir_coef{BMP3_IIR_FILTER_DISABLE};	// IIR coefficient
+    static constexpr uint8_t osr_t{BMP3_OVERSAMPLING_2X};       // oversampling rate, temperature
+    static constexpr uint8_t osr_p{BMP3_OVERSAMPLING_16X};      // oversampling rate, pressure
+    static constexpr uint8_t odr{BMP3_ODR_50_HZ};               // output data rate (not used)
+    static constexpr uint8_t iir_coef{BMP3_IIR_FILTER_DISABLE}; // IIR coefficient
 
-	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
-	IBMP388			*_interface{nullptr};
+    uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+    IBMP388                              *_interface{nullptr};
 
-	unsigned		_measure_interval{0};			// interval in microseconds needed to measure
+    unsigned _measure_interval{0}; // interval in microseconds needed to measure
 
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
+    perf_counter_t _sample_perf;
+    perf_counter_t _measure_perf;
+    perf_counter_t _comms_errors;
 
-	calibration_s		*_cal {nullptr}; // stored calibration constants
+    calibration_s *_cal{nullptr}; // stored calibration constants
 
-	bool			_collect_phase{false};
+    bool _collect_phase{false};
 
-	uint8_t                 _chip_id{0};
-	uint8_t                 _chip_rev_id{0};
+    uint8_t _chip_id{0};
+    uint8_t _chip_rev_id{0};
 
-	void 			start();
-	int 			measure();
-	int			collect(); //get results and publish
-	uint32_t		get_measurement_time();
+    void     start();
+    int      measure();
+    int      collect(); //get results and publish
+    uint32_t get_measurement_time();
 
-	bool			soft_reset();
-	bool			get_calib_data();
-	bool			validate_trimming_param();
-	bool 			set_sensor_settings();
-	bool			set_op_mode(uint8_t op_mode);
+    bool soft_reset();
+    bool get_calib_data();
+    bool validate_trimming_param();
+    bool set_sensor_settings();
+    bool set_op_mode(uint8_t op_mode);
 
-	bool 			get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data);
-	bool			compensate_data(uint8_t sensor_comp, const struct bmp3_uncomp_data *uncomp_data, struct bmp3_data *comp_data);
+    bool get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data);
+    bool compensate_data(uint8_t sensor_comp, const struct bmp3_uncomp_data *uncomp_data, struct bmp3_data *comp_data);
 };
 
-
 /* interface factories */
 extern IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
 extern IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency);
diff --git a/apps/peripheral/sensor/baro/bmp388/bmp388_i2c.cpp b/apps/peripheral/sensor/baro/bmp388/bmp388_i2c.cpp
index 0b735d35df..b532238611 100644
--- a/apps/peripheral/sensor/baro/bmp388/bmp388_i2c.cpp
+++ b/apps/peripheral/sensor/baro/bmp388/bmp388_i2c.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file bmp388_i2c.cpp
@@ -41,76 +18,72 @@
 
 #include "bmp388.h"
 
-class BMP388_I2C: public device::I2C, public IBMP388
-{
+class BMP388_I2C : public device::I2C, public IBMP388 {
 public:
-	BMP388_I2C(uint8_t bus, uint32_t device, int bus_frequency);
-	virtual ~BMP388_I2C() = default;
+    BMP388_I2C(uint8_t bus, uint32_t device, int bus_frequency);
+    virtual ~BMP388_I2C() = default;
+
+    int init();
 
-	int init();
+    uint8_t        get_reg(uint8_t addr);
+    int            get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
+    int            set_reg(uint8_t value, uint8_t addr);
+    calibration_s *get_calibration(uint8_t addr);
 
-	uint8_t get_reg(uint8_t addr);
-	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
-	int set_reg(uint8_t value, uint8_t addr);
-	calibration_s *get_calibration(uint8_t addr);
+    uint32_t get_device_id() const override {
+        return device::I2C::get_device_id();
+    }
 
-	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
+    uint8_t get_device_address() const override {
+        return device::I2C::get_device_address();
+    }
 
-	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
+    void set_device_type(uint8_t devtype);
 
-	void set_device_type(uint8_t devtype);
 private:
-	struct calibration_s _cal;
+    struct calibration_s _cal;
 };
 
-IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency)
-{
-	return new BMP388_I2C(busnum, device, bus_frequency);
+IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency) {
+    return new BMP388_I2C(busnum, device, bus_frequency);
 }
 
 BMP388_I2C::BMP388_I2C(uint8_t bus, uint32_t device, int bus_frequency) :
-	I2C(DRV_BARO_DEVTYPE_BMP388, MODULE_NAME, bus, device, bus_frequency)
-{
+    I2C(DRV_BARO_DEVTYPE_BMP388, MODULE_NAME, bus, device, bus_frequency) {
 }
 
-int BMP388_I2C::init()
-{
-	return I2C::init();
+int BMP388_I2C::init() {
+    return I2C::init();
 }
 
-uint8_t BMP388_I2C::get_reg(uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr), 0};
-	transfer(&cmd[0], 1, &cmd[1], 1);
+uint8_t BMP388_I2C::get_reg(uint8_t addr) {
+    uint8_t cmd[2] = {(uint8_t)(addr), 0};
+    transfer(&cmd[0], 1, &cmd[1], 1);
 
-	return cmd[1];
+    return cmd[1];
 }
 
-int BMP388_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
-{
-	const uint8_t cmd = (uint8_t)(addr);
-	return transfer(&cmd, sizeof(cmd), buf, len);
+int BMP388_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) {
+    const uint8_t cmd = (uint8_t)(addr);
+    return transfer(&cmd, sizeof(cmd), buf, len);
 }
 
-int BMP388_I2C::set_reg(uint8_t value, uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr), value};
-	return transfer(cmd, sizeof(cmd), nullptr, 0);
+int BMP388_I2C::set_reg(uint8_t value, uint8_t addr) {
+    uint8_t cmd[2] = {(uint8_t)(addr), value};
+    return transfer(cmd, sizeof(cmd), nullptr, 0);
 }
 
-calibration_s *BMP388_I2C::get_calibration(uint8_t addr)
-{
-	const uint8_t cmd = (uint8_t)(addr);
+calibration_s *BMP388_I2C::get_calibration(uint8_t addr) {
+    const uint8_t cmd = (uint8_t)(addr);
 
-	if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct calibration_s)) == OK) {
-		return &(_cal);
+    if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct calibration_s)) == OK) {
+        return &(_cal);
 
-	} else {
-		return nullptr;
-	}
+    } else {
+        return nullptr;
+    }
 }
 
-void BMP388_I2C::set_device_type(uint8_t devtype)
-{
-	device::Device::set_device_type(devtype);
+void BMP388_I2C::set_device_type(uint8_t devtype) {
+    device::Device::set_device_type(devtype);
 }
diff --git a/apps/peripheral/sensor/baro/bmp388/bmp388_main.cpp b/apps/peripheral/sensor/baro/bmp388/bmp388_main.cpp
index fb82cc30e8..76b50994a3 100644
--- a/apps/peripheral/sensor/baro/bmp388/bmp388_main.cpp
+++ b/apps/peripheral/sensor/baro/bmp388/bmp388_main.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
@@ -37,83 +14,79 @@
 
 #include "bmp388.h"
 
-void
-BMP388::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("bmp388", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void BMP388::print_usage() {
+    PRINT_MODULE_USAGE_NAME("bmp388", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-I2CSPIDriverBase *BMP388::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	IBMP388 *interface = nullptr;
+I2CSPIDriverBase *BMP388::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    IBMP388 *interface = nullptr;
 
-	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = bmp388_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
+    if (config.bus_type == BOARD_I2C_BUS) {
+        interface = bmp388_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
 
-	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = bmp388_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
-	}
+    } else if (config.bus_type == BOARD_SPI_BUS) {
+        interface = bmp388_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+    }
 
-	if (interface == nullptr) {
-		PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
-		return nullptr;
-	}
+    if (interface == nullptr) {
+        PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
+        return nullptr;
+    }
 
-	if (interface->init() != OK) {
-		delete interface;
-		PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
-		return nullptr;
-	}
+    if (interface->init() != OK) {
+        delete interface;
+        PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
+        return nullptr;
+    }
 
-	BMP388 *dev = new BMP388(config, interface);
+    BMP388 *dev = new BMP388(config, interface);
 
-	if (dev == nullptr) {
-		delete interface;
-		return nullptr;
-	}
+    if (dev == nullptr) {
+        delete interface;
+        return nullptr;
+    }
 
-	if (OK != dev->init()) {
-		delete dev;
-		return nullptr;
-	}
+    if (OK != dev->init()) {
+        delete dev;
+        return nullptr;
+    }
 
-	return dev;
+    return dev;
 }
 
-extern "C" int bmp388_main(int argc, char *argv[])
-{
-	using ThisDriver = BMP388;
-	BusCLIArguments cli{true, true};
-	cli.i2c_address = 0x76;
-	cli.default_i2c_frequency = 100 * 1000;
-	cli.default_spi_frequency = 10 * 1000 * 1000;
+extern "C" int bmp388_main(int argc, char *argv[]) {
+    using ThisDriver = BMP388;
+    BusCLIArguments cli{true, true};
+    cli.i2c_address           = 0x76;
+    cli.default_i2c_frequency = 100 * 1000;
+    cli.default_spi_frequency = 10 * 1000 * 1000;
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+    const char *verb = cli.parseDefaultArguments(argc, argv);
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_BMP388);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_BMP388);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/baro/bmp388/bmp388_spi.cpp b/apps/peripheral/sensor/baro/bmp388/bmp388_spi.cpp
index 18479a4aaf..4e0829a13b 100644
--- a/apps/peripheral/sensor/baro/bmp388/bmp388_spi.cpp
+++ b/apps/peripheral/sensor/baro/bmp388/bmp388_spi.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file bmp388_spi.cpp
@@ -43,91 +20,89 @@
 
 
 /* SPI protocol address bits */
-#define DIR_READ			(1<<7)  //for set
-#define DIR_WRITE			~(1<<7) //for clear
+#define DIR_READ  (1 << 7)  //for set
+#define DIR_WRITE ~(1 << 7) //for clear
+
+#pragma pack(push, 1)
 
-#pragma pack(push,1)
 struct spi_data_s {
-	uint8_t addr;
-	struct data_s data;
+    uint8_t       addr;
+    struct data_s data;
 };
 
 struct spi_calibration_s {
-	uint8_t addr;
-	struct calibration_s cal;
+    uint8_t              addr;
+    struct calibration_s cal;
 };
+
 #pragma pack(pop)
 
-class BMP388_SPI: public device::SPI, public IBMP388
-{
+class BMP388_SPI : public device::SPI, public IBMP388 {
 public:
-	BMP388_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
-	virtual ~BMP388_SPI() = default;
+    BMP388_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+    virtual ~BMP388_SPI() = default;
+
+    int init();
 
-	int init();
+    uint8_t        get_reg(uint8_t addr);
+    int            get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
+    int            set_reg(uint8_t value, uint8_t addr);
+    calibration_s *get_calibration(uint8_t addr);
 
-	uint8_t get_reg(uint8_t addr);
-	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
-	int set_reg(uint8_t value, uint8_t addr);
-	calibration_s *get_calibration(uint8_t addr);
+    uint32_t get_device_id() const override {
+        return device::SPI::get_device_id();
+    }
 
-	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
+    uint8_t get_device_address() const override {
+        return device::SPI::get_device_address();
+    }
 
-	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
+    void set_device_type(uint8_t devtype);
 
-	void set_device_type(uint8_t devtype);
 private:
-	spi_calibration_s _cal;
+    spi_calibration_s _cal;
 };
 
-IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
-{
-	return new BMP388_SPI(busnum, device, bus_frequency, spi_mode);
+IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode) {
+    return new BMP388_SPI(busnum, device, bus_frequency, spi_mode);
 }
 
 BMP388_SPI::BMP388_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_BARO_DEVTYPE_BMP388, MODULE_NAME, bus, device, spi_mode, bus_frequency)
-{
+    SPI(DRV_BARO_DEVTYPE_BMP388, MODULE_NAME, bus, device, spi_mode, bus_frequency) {
 }
 
-int BMP388_SPI::init()
-{
-	return SPI::init();
+int BMP388_SPI::init() {
+    return SPI::init();
 };
 
-uint8_t BMP388_SPI::get_reg(uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit
-	transfer(&cmd[0], &cmd[0], 2);
+uint8_t BMP388_SPI::get_reg(uint8_t addr) {
+    uint8_t cmd[2] = {(uint8_t)(addr | DIR_READ), 0}; //set MSB bit
+    transfer(&cmd[0], &cmd[0], 2);
 
-	return cmd[1];
+    return cmd[1];
 }
 
-int BMP388_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
-{
-	uint8_t cmd[1] = {(uint8_t)(addr | DIR_READ)};
-	return transfer(&cmd[0], buf, len);
+int BMP388_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) {
+    uint8_t cmd[1] = {(uint8_t)(addr | DIR_READ)};
+    return transfer(&cmd[0], buf, len);
 }
 
-int BMP388_SPI::set_reg(uint8_t value, uint8_t addr)
-{
-	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit
-	return transfer(&cmd[0], nullptr, 2);
+int BMP388_SPI::set_reg(uint8_t value, uint8_t addr) {
+    uint8_t cmd[2] = {(uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit
+    return transfer(&cmd[0], nullptr, 2);
 }
 
-calibration_s *BMP388_SPI::get_calibration(uint8_t addr)
-{
-	_cal.addr = addr | DIR_READ;
+calibration_s *BMP388_SPI::get_calibration(uint8_t addr) {
+    _cal.addr = addr | DIR_READ;
 
-	if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) {
-		return &(_cal.cal);
+    if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) {
+        return &(_cal.cal);
 
-	} else {
-		return nullptr;
-	}
+    } else {
+        return nullptr;
+    }
 }
 
-void BMP388_SPI::set_device_type(uint8_t devtype)
-{
-	device::Device::set_device_type(devtype);
+void BMP388_SPI::set_device_type(uint8_t devtype) {
+    device::Device::set_device_type(devtype);
 }
diff --git a/apps/peripheral/sensor/baro/ms5611/MS5611.hpp b/apps/peripheral/sensor/baro/ms5611/MS5611.hpp
index 6f297e86b4..8cf8cd854c 100644
--- a/apps/peripheral/sensor/baro/ms5611/MS5611.hpp
+++ b/apps/peripheral/sensor/baro/ms5611/MS5611.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -44,29 +21,35 @@
 #include "ms5611.h"
 
 enum MS56XX_DEVICE_TYPES {
-	MS5611_DEVICE	= 5611,
-	MS5607_DEVICE	= 5607,
+    MS5611_DEVICE = 5611,
+    MS5607_DEVICE = 5607,
 };
 
 /* helper macro for handling report buffer indices */
-#define INCREMENT(_x, _lim)	do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
+#define INCREMENT(_x, _lim)           \
+    do {                              \
+        __typeof__(_x) _tmp = _x + 1; \
+        if (_tmp >= _lim)             \
+            _tmp = 0;                 \
+        _x = _tmp;                    \
+    } while (0)
 
 /* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
+#define POW2(_x) ((_x) * (_x))
 
 /*
  * MS5611/MS5607 internal constants and data structures.
  */
-#define ADDR_CMD_CONVERT_D1_OSR256		0x40	/* write to this address to start pressure conversion */
-#define ADDR_CMD_CONVERT_D1_OSR512		0x42	/* write to this address to start pressure conversion */
-#define ADDR_CMD_CONVERT_D1_OSR1024		0x44	/* write to this address to start pressure conversion */
-#define ADDR_CMD_CONVERT_D1_OSR2048		0x46	/* write to this address to start pressure conversion */
-#define ADDR_CMD_CONVERT_D1_OSR4096		0x48	/* write to this address to start pressure conversion */
-#define ADDR_CMD_CONVERT_D2_OSR256		0x50	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2_OSR512		0x52	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2_OSR1024		0x54	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2_OSR2048		0x56	/* write to this address to start temperature conversion */
-#define ADDR_CMD_CONVERT_D2_OSR4096		0x58	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D1_OSR256  0x40 /* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR512  0x42 /* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */
+#define ADDR_CMD_CONVERT_D2_OSR256  0x50 /* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR512  0x52 /* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */
 
 /*
   use an OSR of 1024 to reduce the self-heating effect of the
@@ -74,29 +57,28 @@ enum MS56XX_DEVICE_TYPES {
   are quite sensitive to this effect and that reducing the OSR can
   make a big difference
  */
-#define ADDR_CMD_CONVERT_D1			ADDR_CMD_CONVERT_D1_OSR1024
-#define ADDR_CMD_CONVERT_D2			ADDR_CMD_CONVERT_D2_OSR1024
+#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024
+#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024
 
 /*
  * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update
  * rate of 100Hz which is be very safe not to read the ADC before the
  * conversion finished
  */
-#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
+#define MS5611_MEASUREMENT_RATIO   3     /* pressure measurements per temperature measurement */
 
-class MS5611 : public I2CSPIDriver<MS5611>
-{
+class MS5611 : public I2CSPIDriver<MS5611> {
 public:
-	MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config);
-	~MS5611() override;
+    MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config);
+    ~MS5611() override;
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	int		init();
+    int init();
 
-	/**
+    /**
 	 * Perform a poll cycle; collect from the previous measurement
 	 * and start a new one.
 	 *
@@ -109,51 +91,51 @@ class MS5611 : public I2CSPIDriver<MS5611>
 	 * and measurement to provide the most recent measurement possible
 	 * at the next interval.
 	 */
-	void			RunImpl();
+    void RunImpl();
 
 protected:
-	void print_status() override;
+    void print_status() override;
 
-	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+    uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
-	device::Device		*_interface;
+    device::Device *_interface;
 
-	ms5611::prom_s		_prom;
+    ms5611::prom_s _prom;
 
-	enum MS56XX_DEVICE_TYPES _device_type;
-	bool			_collect_phase{false};
-	unsigned		_measure_phase{false};
+    enum MS56XX_DEVICE_TYPES _device_type;
+    bool                     _collect_phase{false};
+    unsigned                 _measure_phase{false};
 
-	/* intermediate temperature values per MS5611/MS5607 datasheet */
-	int64_t			_OFF{0};
-	int64_t			_SENS{0};
+    /* intermediate temperature values per MS5611/MS5607 datasheet */
+    int64_t _OFF{0};
+    int64_t _SENS{0};
 
-	bool _initialized{false};
+    bool _initialized{false};
 
-	float _last_pressure{NAN};
-	float _last_temperature{NAN};
+    float _last_pressure{NAN};
+    float _last_temperature{NAN};
 
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
+    perf_counter_t _sample_perf;
+    perf_counter_t _measure_perf;
+    perf_counter_t _comms_errors;
 
-	/**
+    /**
 	 * Initialize the automatic measurement state machine and start it.
 	 *
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
 	 */
-	void			start();
+    void start();
 
-	/**
+    /**
 	 * Issue a measurement command for the current state.
 	 *
 	 * @return		OK if the measurement command was successful.
 	 */
-	int			measure();
+    int measure();
 
-	/**
+    /**
 	 * Collect the result of the most recent measurement.
 	 */
-	int			collect();
+    int collect();
 };
diff --git a/apps/peripheral/sensor/baro/ms5611/ms5611.cpp b/apps/peripheral/sensor/baro/ms5611/ms5611.cpp
index 871316a0fd..fa5b35420b 100644
--- a/apps/peripheral/sensor/baro/ms5611/ms5611.cpp
+++ b/apps/peripheral/sensor/baro/ms5611/ms5611.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ms5611.cpp
@@ -42,377 +19,355 @@
 #include <cdev/CDev.hpp>
 
 MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config) :
-	I2CSPIDriver(config),
-	_interface(interface),
-	_prom(prom_buf.s),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
-{
-	switch (config.devid_driver_index) {
-	case DRV_BARO_DEVTYPE_MS5611:
-		_device_type = MS5611_DEVICE;
-		break;
-
-	case DRV_BARO_DEVTYPE_MS5607:
-	default:
-		_device_type = MS5607_DEVICE;
-		break;
-	}
+    I2CSPIDriver(config),
+    _interface(interface),
+    _prom(prom_buf.s),
+    _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": read")),
+    _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": measure")),
+    _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME ": com_err")) {
+    switch (config.devid_driver_index) {
+    case DRV_BARO_DEVTYPE_MS5611:
+        _device_type = MS5611_DEVICE;
+        break;
+
+    case DRV_BARO_DEVTYPE_MS5607:
+    default:
+        _device_type = MS5607_DEVICE;
+        break;
+    }
 }
 
-MS5611::~MS5611()
-{
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_measure_perf);
-	perf_free(_comms_errors);
+MS5611::~MS5611() {
+    // free perf counters
+    perf_free(_sample_perf);
+    perf_free(_measure_perf);
+    perf_free(_comms_errors);
 
-	delete _interface;
+    delete _interface;
 }
 
-int
-MS5611::init()
-{
-	int ret;
-
-	/* do a first measurement cycle to populate reports with valid data */
-	_measure_phase = 0;
-
-	while (true) {
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		px4_usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* now do a pressure measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		px4_usleep(MS5611_CONVERSION_INTERVAL);
-
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		if (_device_type == MS5607_DEVICE) {
-			if (_last_pressure < 52'000.f) {
-				/* This is likely not this device, abort */
-				ret = -EINVAL;
-				break;
-
-			} else if (_last_pressure > 150'000.f) {
-				/* This is likely not this device, abort */
-				ret = -EINVAL;
-				break;
-			}
-		}
-
-		switch (_device_type) {
-		default:
-
-		/* fall through */
-		case MS5611_DEVICE:
-			_interface->set_device_type(DRV_BARO_DEVTYPE_MS5611);
-			break;
-
-		case MS5607_DEVICE:
-			_interface->set_device_type(DRV_BARO_DEVTYPE_MS5607);
-			break;
-		}
-
-		ret = OK;
-
-		break;
-	}
-
-	if (ret == 0) {
-		_initialized = true;
-		start();
-	}
-
-	return ret;
+int MS5611::init() {
+    int ret;
+
+    /* do a first measurement cycle to populate reports with valid data */
+    _measure_phase = 0;
+
+    while (true) {
+        /* do temperature first */
+        if (OK != measure()) {
+            ret = -EIO;
+            break;
+        }
+
+        px4_usleep(MS5611_CONVERSION_INTERVAL);
+
+        if (OK != collect()) {
+            ret = -EIO;
+            break;
+        }
+
+        /* now do a pressure measurement */
+        if (OK != measure()) {
+            ret = -EIO;
+            break;
+        }
+
+        px4_usleep(MS5611_CONVERSION_INTERVAL);
+
+        if (OK != collect()) {
+            ret = -EIO;
+            break;
+        }
+
+        /* state machine will have generated a report, copy it out */
+        if (_device_type == MS5607_DEVICE) {
+            if (_last_pressure < 52'000.f) {
+                /* This is likely not this device, abort */
+                ret = -EINVAL;
+                break;
+
+            } else if (_last_pressure > 150'000.f) {
+                /* This is likely not this device, abort */
+                ret = -EINVAL;
+                break;
+            }
+        }
+
+        switch (_device_type) {
+        default:
+
+        /* fall through */
+        case MS5611_DEVICE:
+            _interface->set_device_type(DRV_BARO_DEVTYPE_MS5611);
+            break;
+
+        case MS5607_DEVICE:
+            _interface->set_device_type(DRV_BARO_DEVTYPE_MS5607);
+            break;
+        }
+
+        ret = OK;
+
+        break;
+    }
+
+    if (ret == 0) {
+        _initialized = true;
+        start();
+    }
+
+    return ret;
 }
 
-void
-MS5611::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_measure_phase = 0;
+void MS5611::start() {
+    /* reset the report ring and state machine */
+    _collect_phase = false;
+    _measure_phase = 0;
 
-	/* schedule a cycle to start things */
-	ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
+    /* schedule a cycle to start things */
+    ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
 }
 
-void
-MS5611::RunImpl()
-{
-	int ret;
-	unsigned dummy;
+void MS5611::RunImpl() {
+    int      ret;
+    unsigned dummy;
 
-	/* collection phase? */
-	if (_collect_phase) {
+    /* collection phase? */
+    if (_collect_phase) {
+        /* perform collection */
+        ret = collect();
 
-		/* perform collection */
-		ret = collect();
-
-		if (ret != OK) {
-			if (ret == -6) {
-				/*
+        if (ret != OK) {
+            if (ret == -6) {
+                /*
 				 * The ms5611 seems to regularly fail to respond to
 				 * its address; this happens often enough that we'd rather not
 				 * spam the console with a message for this.
 				 */
-			} else {
-				//DEVICE_LOG("collection error %d", ret);
-			}
+            } else {
+                //DEVICE_LOG("collection error %d", ret);
+            }
 
-			/* issue a reset command to the sensor */
-			_interface->ioctl(IOCTL_RESET, dummy);
-			/* reset the collection state machine and try again - we need
+            /* issue a reset command to the sensor */
+            _interface->ioctl(IOCTL_RESET, dummy);
+            /* reset the collection state machine and try again - we need
 			 * to wait 2.8 ms after issuing the sensor reset command
 			 * according to the MS5611 datasheet
 			 */
-			ScheduleDelayed(2800);
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-	}
-
-	/* measurement phase */
-	ret = measure();
-
-	if (ret != OK) {
-		/* issue a reset command to the sensor */
-		_interface->ioctl(IOCTL_RESET, dummy);
-		/* reset the collection state machine and try again */
-		start();
-		return;
-	}
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
+            ScheduleDelayed(2800);
+            return;
+        }
+
+        /* next phase is measurement */
+        _collect_phase = false;
+    }
+
+    /* measurement phase */
+    ret = measure();
+
+    if (ret != OK) {
+        /* issue a reset command to the sensor */
+        _interface->ioctl(IOCTL_RESET, dummy);
+        /* reset the collection state machine and try again */
+        start();
+        return;
+    }
+
+    /* next phase is collection */
+    _collect_phase = true;
+
+    /* schedule a fresh cycle call when the measurement is done */
+    ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
 }
 
-int
-MS5611::measure()
-{
-	perf_begin(_measure_perf);
+int MS5611::measure() {
+    perf_begin(_measure_perf);
 
-	/*
+    /*
 	 * In phase zero, request temperature; in other phases, request pressure.
 	 */
-	unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+    unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
 
-	/*
+    /*
 	 * Send the command to begin measuring.
 	 */
-	int ret = _interface->ioctl(IOCTL_MEASURE, addr);
+    int ret = _interface->ioctl(IOCTL_MEASURE, addr);
 
-	if (OK != ret) {
-		perf_count(_comms_errors);
-	}
+    if (OK != ret) {
+        perf_count(_comms_errors);
+    }
 
-	perf_end(_measure_perf);
+    perf_end(_measure_perf);
 
-	return ret;
+    return ret;
 }
 
-int
-MS5611::collect()
-{
-	uint32_t raw;
-
-	perf_begin(_sample_perf);
-
-	/* read the most recent measurement - read offset/size are hardcoded in the interface */
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-	int ret = _interface->read(0, (void *)&raw, 0);
-
-	if (ret < 0) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	// According to the sensor docs:
-	// If the conversion is not executed before the ADC read command, or the
-	// ADC read command is repeated, it will give 0 as the output result.
-	//
-	// We have seen 0 during the init phase on I2C, therefore, we add this
-	// protection in.
-	if (raw == 0) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	/* handle a measurement */
-	if (_measure_phase == 0) {
-
-		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
-
-		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
-
-		/* base sensor scale/offset values */
-		if (_device_type == MS5611_DEVICE) {
-			/* Perform MS5611 Caculation */
-			_OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
-			_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
-
-			/* MS5611 temperature compensation */
-			if (TEMP < 2000) {
-
-				int32_t T2 = POW2(dT) >> 31;
-
-				int64_t f = POW2((int64_t)TEMP - 2000);
-				int64_t OFF2 = 5 * f >> 1;
-				int64_t SENS2 = 5 * f >> 2;
-
-				if (TEMP < -1500) {
-
-					int64_t f2 = POW2(TEMP + 1500);
-					OFF2 += 7 * f2;
-					SENS2 += 11 * f2 >> 1;
-				}
-
-				TEMP -= T2;
-				_OFF  -= OFF2;
-				_SENS -= SENS2;
-			}
-
-		} else if (_device_type == MS5607_DEVICE) {
-			/* Perform MS5607 Caculation */
-			_OFF  = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6);
-			_SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7);
-
-			/* MS5607 temperature compensation */
-			if (TEMP < 2000) {
-
-				int32_t T2 = POW2(dT) >> 31;
-
-				int64_t f = POW2((int64_t)TEMP - 2000);
-				int64_t OFF2 = 61 * f >> 4;
-				int64_t SENS2 = 2 * f;
-
-				if (TEMP < -1500) {
-					int64_t f2 = POW2(TEMP + 1500);
-					OFF2 += 15 * f2;
-					SENS2 += 8 * f2;
-				}
-
-				TEMP -= T2;
-				_OFF  -= OFF2;
-				_SENS -= SENS2;
-			}
-		}
-
-		_last_temperature = TEMP / 100.0f;
-
-	} else {
-		/* pressure calculation, result in Pa */
-		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
-
-		_last_pressure = P;
-
-		// publish
-		if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) {
-			sensor_baro_s sensor_baro{};
-			sensor_baro.timestamp_sample = timestamp_sample;
-			sensor_baro.device_id = _interface->get_device_id();
-			sensor_baro.pressure = P;
-			sensor_baro.temperature = _last_temperature;
-			sensor_baro.error_count = perf_event_count(_comms_errors);
-			sensor_baro.timestamp = hrt_absolute_time();
-			_sensor_baro_pub.publish(sensor_baro);
-		}
-
-	}
-
-	/* update the measurement state machine */
-	INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
-
-	perf_end(_sample_perf);
-
-	return OK;
+int MS5611::collect() {
+    uint32_t raw;
+
+    perf_begin(_sample_perf);
+
+    /* read the most recent measurement - read offset/size are hardcoded in the interface */
+    const hrt_abstime timestamp_sample = hrt_absolute_time();
+    int               ret              = _interface->read(0, (void *)&raw, 0);
+
+    if (ret < 0) {
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return ret;
+    }
+
+    // According to the sensor docs:
+    // If the conversion is not executed before the ADC read command, or the
+    // ADC read command is repeated, it will give 0 as the output result.
+    //
+    // We have seen 0 during the init phase on I2C, therefore, we add this
+    // protection in.
+    if (raw == 0) {
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return ret;
+    }
+
+    /* handle a measurement */
+    if (_measure_phase == 0) {
+        /* temperature offset (in ADC units) */
+        int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
+
+        /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
+        int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
+
+        /* base sensor scale/offset values */
+        if (_device_type == MS5611_DEVICE) {
+            /* Perform MS5611 Caculation */
+            _OFF  = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
+            _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
+
+            /* MS5611 temperature compensation */
+            if (TEMP < 2000) {
+                int32_t T2 = POW2(dT) >> 31;
+
+                int64_t f     = POW2((int64_t)TEMP - 2000);
+                int64_t OFF2  = 5 * f >> 1;
+                int64_t SENS2 = 5 * f >> 2;
+
+                if (TEMP < -1500) {
+                    int64_t f2  = POW2(TEMP + 1500);
+                    OFF2       += 7 * f2;
+                    SENS2      += 11 * f2 >> 1;
+                }
+
+                TEMP  -= T2;
+                _OFF  -= OFF2;
+                _SENS -= SENS2;
+            }
+
+        } else if (_device_type == MS5607_DEVICE) {
+            /* Perform MS5607 Caculation */
+            _OFF  = ((int64_t)_prom.c2_pressure_offset << 17) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 6);
+            _SENS = ((int64_t)_prom.c1_pressure_sens << 16) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 7);
+
+            /* MS5607 temperature compensation */
+            if (TEMP < 2000) {
+                int32_t T2 = POW2(dT) >> 31;
+
+                int64_t f     = POW2((int64_t)TEMP - 2000);
+                int64_t OFF2  = 61 * f >> 4;
+                int64_t SENS2 = 2 * f;
+
+                if (TEMP < -1500) {
+                    int64_t f2  = POW2(TEMP + 1500);
+                    OFF2       += 15 * f2;
+                    SENS2      += 8 * f2;
+                }
+
+                TEMP  -= T2;
+                _OFF  -= OFF2;
+                _SENS -= SENS2;
+            }
+        }
+
+        _last_temperature = TEMP / 100.0f;
+
+    } else {
+        /* pressure calculation, result in Pa */
+        int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+
+        _last_pressure = P;
+
+        // publish
+        if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) {
+            sensor_baro_s sensor_baro{};
+            sensor_baro.timestamp_sample = timestamp_sample;
+            sensor_baro.device_id        = _interface->get_device_id();
+            sensor_baro.pressure         = P;
+            sensor_baro.temperature      = _last_temperature;
+            sensor_baro.error_count      = perf_event_count(_comms_errors);
+            sensor_baro.timestamp        = hrt_absolute_time();
+            _sensor_baro_pub.publish(sensor_baro);
+        }
+    }
+
+    /* update the measurement state machine */
+    INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
+
+    perf_end(_sample_perf);
+
+    return OK;
 }
 
-void MS5611::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+void MS5611::print_status() {
+    I2CSPIDriverBase::print_status();
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
 
-	printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
+    printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
 }
 
-namespace ms5611
-{
+namespace ms5611 {
 
 /**
  * MS5611 crc4 cribbed from the datasheet
  */
-bool
-crc4(uint16_t *n_prom)
-{
-	int16_t cnt;
-	uint16_t n_rem;
-	uint16_t crc_read;
-	uint8_t n_bit;
-
-	n_rem = 0x00;
-
-	/* save the read crc */
-	crc_read = n_prom[7];
-
-	/* remove CRC byte */
-	n_prom[7] = (0xFF00 & (n_prom[7]));
-
-	for (cnt = 0; cnt < 16; cnt++) {
-		/* uneven bytes */
-		if (cnt & 1) {
-			n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
-
-		} else {
-			n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
-		}
-
-		for (n_bit = 8; n_bit > 0; n_bit--) {
-			if (n_rem & 0x8000) {
-				n_rem = (n_rem << 1) ^ 0x3000;
-
-			} else {
-				n_rem = (n_rem << 1);
-			}
-		}
-	}
-
-	/* final 4 bit remainder is CRC value */
-	n_rem = (0x000F & (n_rem >> 12));
-	n_prom[7] = crc_read;
-
-	/* return true if CRCs match */
-	return (0x000F & crc_read) == (n_rem ^ 0x00);
+bool crc4(uint16_t *n_prom) {
+    int16_t  cnt;
+    uint16_t n_rem;
+    uint16_t crc_read;
+    uint8_t  n_bit;
+
+    n_rem = 0x00;
+
+    /* save the read crc */
+    crc_read = n_prom[7];
+
+    /* remove CRC byte */
+    n_prom[7] = (0xFF00 & (n_prom[7]));
+
+    for (cnt = 0; cnt < 16; cnt++) {
+        /* uneven bytes */
+        if (cnt & 1) {
+            n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
+
+        } else {
+            n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
+        }
+
+        for (n_bit = 8; n_bit > 0; n_bit--) {
+            if (n_rem & 0x8000) {
+                n_rem = (n_rem << 1) ^ 0x3000;
+
+            } else {
+                n_rem = (n_rem << 1);
+            }
+        }
+    }
+
+    /* final 4 bit remainder is CRC value */
+    n_rem     = (0x000F & (n_rem >> 12));
+    n_prom[7] = crc_read;
+
+    /* return true if CRCs match */
+    return (0x000F & crc_read) == (n_rem ^ 0x00);
 }
 
 } // namespace ms5611
diff --git a/apps/peripheral/sensor/baro/ms5611/ms5611.h b/apps/peripheral/sensor/baro/ms5611/ms5611.h
index c8efaa7eca..e9b785a0ea 100644
--- a/apps/peripheral/sensor/baro/ms5611/ms5611.h
+++ b/apps/peripheral/sensor/baro/ms5611/ms5611.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ms5611.h
@@ -44,7 +21,7 @@
 #include <string.h>
 
 #if defined(CONFIG_I2C)
-# include <drivers/device/i2c.h>
+#   include <drivers/device/i2c.h>
 #endif // CONFIG_I2C
 
 #include <drivers/device/device.h>
@@ -53,55 +30,55 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <systemlib/err.h>
 
-#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
-#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_RESET_CMD  0x1E /* write to this address to reset chip */
+#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
 
 /* interface ioctls */
-#define IOCTL_RESET			2
-#define IOCTL_MEASURE			3
+#define IOCTL_RESET      2
+#define IOCTL_MEASURE    3
 
-#define MS5611_ADDRESS_1		0x76	/* address select pins pulled high (PX4FMU series v1.6+) */
-#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
 
-
-namespace ms5611
-{
+namespace ms5611 {
 
 /**
  * Calibration PROM as reported by the device.
  */
-#pragma pack(push,1)
+#pragma pack(push, 1)
+
 struct prom_s {
-	uint16_t factory_setup;
-	uint16_t c1_pressure_sens;
-	uint16_t c2_pressure_offset;
-	uint16_t c3_temp_coeff_pres_sens;
-	uint16_t c4_temp_coeff_pres_offset;
-	uint16_t c5_reference_temp;
-	uint16_t c6_temp_coeff_temp;
-	uint16_t serial_and_crc;
+    uint16_t factory_setup;
+    uint16_t c1_pressure_sens;
+    uint16_t c2_pressure_offset;
+    uint16_t c3_temp_coeff_pres_sens;
+    uint16_t c4_temp_coeff_pres_offset;
+    uint16_t c5_reference_temp;
+    uint16_t c6_temp_coeff_temp;
+    uint16_t serial_and_crc;
 };
 
 /**
  * Grody hack for crc4()
  */
 union prom_u {
-	uint16_t c[8];
-	prom_s s;
+    uint16_t c[8];
+    prom_s   s;
 };
+
 #pragma pack(pop)
 
 extern bool crc4(uint16_t *n_prom);
 
-} /* namespace */
+} // namespace ms5611
 
 /* interface factories */
 extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency,
-		spi_mode_e spi_mode);
+                                            spi_mode_e spi_mode);
 
 #if defined(CONFIG_I2C)
 extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum,
-		int bus_frequency);
+                                            int bus_frequency);
 #endif // CONFIG_I2C
 
 typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum);
diff --git a/apps/peripheral/sensor/baro/ms5611/ms5611_i2c.cpp b/apps/peripheral/sensor/baro/ms5611/ms5611_i2c.cpp
index 30ddc54abb..e42974a102 100644
--- a/apps/peripheral/sensor/baro/ms5611/ms5611_i2c.cpp
+++ b/apps/peripheral/sensor/baro/ms5611/ms5611_i2c.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ms5611_i2c.cpp
@@ -41,204 +18,186 @@
 
 #if defined(CONFIG_I2C)
 
-#include "ms5611.h"
+#   include "ms5611.h"
 
-class MS5611_I2C : public device::I2C
-{
+class MS5611_I2C : public device::I2C {
 public:
-	MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf, int bus_frequency);
-	~MS5611_I2C() override = default;
+    MS5611_I2C(uint8_t bus, ms5611::prom_u &prom_buf, int bus_frequency);
+    ~MS5611_I2C() override = default;
 
-	int	read(unsigned offset, void *data, unsigned count) override;
-	int	ioctl(unsigned operation, unsigned &arg) override;
+    int read(unsigned offset, void *data, unsigned count) override;
+    int ioctl(unsigned operation, unsigned &arg) override;
 
 protected:
-	int	probe() override;
+    int probe() override;
 
 private:
-	ms5611::prom_u	&_prom;
+    ms5611::prom_u &_prom;
 
-	int		_probe_address(uint8_t address);
+    int _probe_address(uint8_t address);
 
-	/**
+    /**
 	 * Send a reset command to the MS5611.
 	 *
 	 * This is required after any bus reset.
 	 */
-	int		_reset();
+    int _reset();
 
-	/**
+    /**
 	 * Send a measure command to the MS5611.
 	 *
 	 * @param addr		Which address to use for the measure operation.
 	 */
-	int		_measure(unsigned addr);
+    int _measure(unsigned addr);
 
-	/**
+    /**
 	 * Read the MS5611 PROM
 	 *
 	 * @return		PX4_OK if the PROM reads successfully.
 	 */
-	int		_read_prom();
-
+    int _read_prom();
 };
 
 device::Device *
-MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency)
-{
-	return new MS5611_I2C(busnum, prom_buf, bus_frequency);
+MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency) {
+    return new MS5611_I2C(busnum, prom_buf, bus_frequency);
 }
 
 MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom, int bus_frequency) :
-	I2C(DRV_BARO_DEVTYPE_MS5611, MODULE_NAME, bus, 0, bus_frequency),
-	_prom(prom)
-{
+    I2C(DRV_BARO_DEVTYPE_MS5611, MODULE_NAME, bus, 0, bus_frequency),
+    _prom(prom) {
 }
 
-int
-MS5611_I2C::read(unsigned offset, void *data, unsigned count)
-{
-	union _cvt {
-		uint8_t	b[4];
-		uint32_t w;
-	} *cvt = (_cvt *)data;
-	uint8_t buf[3];
-
-	/* read the most recent measurement */
-	uint8_t cmd = 0;
-	int ret = transfer(&cmd, 1, &buf[0], 3);
-
-	if (ret == PX4_OK) {
-		/* fetch the raw value */
-		cvt->b[0] = buf[2];
-		cvt->b[1] = buf[1];
-		cvt->b[2] = buf[0];
-		cvt->b[3] = 0;
-	}
-
-	return ret;
+int MS5611_I2C::read(unsigned offset, void *data, unsigned count) {
+    union _cvt {
+        uint8_t  b[4];
+        uint32_t w;
+    } *cvt = (_cvt *)data;
+
+    uint8_t buf[3];
+
+    /* read the most recent measurement */
+    uint8_t cmd = 0;
+    int     ret = transfer(&cmd, 1, &buf[0], 3);
+
+    if (ret == PX4_OK) {
+        /* fetch the raw value */
+        cvt->b[0] = buf[2];
+        cvt->b[1] = buf[1];
+        cvt->b[2] = buf[0];
+        cvt->b[3] = 0;
+    }
+
+    return ret;
 }
 
-int
-MS5611_I2C::ioctl(unsigned operation, unsigned &arg)
-{
-	int ret;
+int MS5611_I2C::ioctl(unsigned operation, unsigned &arg) {
+    int ret;
 
-	switch (operation) {
-	case IOCTL_RESET:
-		ret = _reset();
-		break;
+    switch (operation) {
+    case IOCTL_RESET:
+        ret = _reset();
+        break;
 
-	case IOCTL_MEASURE:
-		ret = _measure(arg);
-		break;
+    case IOCTL_MEASURE:
+        ret = _measure(arg);
+        break;
 
-	default:
-		ret = EINVAL;
-	}
+    default:
+        ret = EINVAL;
+    }
 
-	return ret;
+    return ret;
 }
 
-int
-MS5611_I2C::probe()
-{
-	if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) ||
-	    (PX4_OK == _probe_address(MS5611_ADDRESS_2))) {
+int MS5611_I2C::probe() {
+    if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) || (PX4_OK == _probe_address(MS5611_ADDRESS_2))) {
+        return PX4_OK;
+    }
 
-		return PX4_OK;
-	}
+    _retries = 1;
 
-	_retries = 1;
-
-	return -EIO;
+    return -EIO;
 }
 
-int
-MS5611_I2C::_probe_address(uint8_t address)
-{
-	/* select the address we are going to try */
-	set_device_address(address);
+int MS5611_I2C::_probe_address(uint8_t address) {
+    /* select the address we are going to try */
+    set_device_address(address);
 
-	/* send reset command */
-	if (PX4_OK != _reset()) {
-		return -EIO;
-	}
+    /* send reset command */
+    if (PX4_OK != _reset()) {
+        return -EIO;
+    }
 
-	/* read PROM */
-	if (PX4_OK != _read_prom()) {
-		return -EIO;
-	}
+    /* read PROM */
+    if (PX4_OK != _read_prom()) {
+        return -EIO;
+    }
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-int
-MS5611_I2C::_reset()
-{
-	unsigned	old_retrycount = _retries;
-	uint8_t		cmd = ADDR_RESET_CMD;
-	int		result;
+int MS5611_I2C::_reset() {
+    unsigned old_retrycount = _retries;
+    uint8_t  cmd            = ADDR_RESET_CMD;
+    int      result;
 
-	/* bump the retry count */
-	_retries = 3;
-	result = transfer(&cmd, 1, nullptr, 0);
-	_retries = old_retrycount;
+    /* bump the retry count */
+    _retries = 3;
+    result   = transfer(&cmd, 1, nullptr, 0);
+    _retries = old_retrycount;
 
-	return result;
+    return result;
 }
 
-int
-MS5611_I2C::_measure(unsigned addr)
-{
-	uint8_t cmd = addr;
-	return transfer(&cmd, 1, nullptr, 0);
+int MS5611_I2C::_measure(unsigned addr) {
+    uint8_t cmd = addr;
+    return transfer(&cmd, 1, nullptr, 0);
 }
 
-int
-MS5611_I2C::_read_prom()
-{
-	uint8_t		prom_buf[2];
-	union {
-		uint8_t		b[2];
-		uint16_t	w;
-	} cvt;
+int MS5611_I2C::_read_prom() {
+    uint8_t prom_buf[2];
+
+    union {
+        uint8_t  b[2];
+        uint16_t w;
+    } cvt;
 
-	/*
+    /*
 	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
 	 * called immediately after reset.
 	 */
-	px4_usleep(3000);
+    px4_usleep(3000);
 
-	uint8_t last_val = 0;
-	bool bits_stuck = true;
+    uint8_t last_val   = 0;
+    bool    bits_stuck = true;
 
-	/* read and convert PROM words */
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+    /* read and convert PROM words */
+    for (int i = 0; i < 8; i++) {
+        uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
 
-		if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
-			break;
-		}
+        if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) {
+            break;
+        }
 
-		/* check if all bytes are zero */
-		if (i == 0) {
-			/* initialize to first byte read */
-			last_val = prom_buf[0];
-		}
+        /* check if all bytes are zero */
+        if (i == 0) {
+            /* initialize to first byte read */
+            last_val = prom_buf[0];
+        }
 
-		if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) {
-			bits_stuck = false;
-		}
+        if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) {
+            bits_stuck = false;
+        }
 
-		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		cvt.b[0] = prom_buf[1];
-		cvt.b[1] = prom_buf[0];
-		_prom.c[i] = cvt.w;
-	}
+        /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+        cvt.b[0]   = prom_buf[1];
+        cvt.b[1]   = prom_buf[0];
+        _prom.c[i] = cvt.w;
+    }
 
-	/* calculate CRC and return success/failure accordingly */
-	return (ms5611::crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO;
+    /* calculate CRC and return success/failure accordingly */
+    return (ms5611::crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO;
 }
 
 #endif // CONFIG_I2C
diff --git a/apps/peripheral/sensor/baro/ms5611/ms5611_main.cpp b/apps/peripheral/sensor/baro/ms5611/ms5611_main.cpp
index 64cbc76b82..48bd08f865 100644
--- a/apps/peripheral/sensor/baro/ms5611/ms5611_main.cpp
+++ b/apps/peripheral/sensor/baro/ms5611/ms5611_main.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
@@ -40,113 +17,109 @@
 #include "MS5611.hpp"
 #include "ms5611.h"
 
-I2CSPIDriverBase *MS5611::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	ms5611::prom_u prom_buf;
-	device::Device *interface = nullptr;
+I2CSPIDriverBase *MS5611::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    ms5611::prom_u  prom_buf;
+    device::Device *interface = nullptr;
 
 #if defined(CONFIG_I2C)
 
-	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = MS5611_i2c_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency);
+    if (config.bus_type == BOARD_I2C_BUS) {
+        interface = MS5611_i2c_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency);
 
-	} else
+    } else
 #endif // CONFIG_I2C
-		if (config.bus_type == BOARD_SPI_BUS) {
-			interface = MS5611_spi_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency, config.spi_mode);
-		}
-
-	if (interface == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-
-	if (interface->init() != OK) {
-		delete interface;
-		PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid);
-		return nullptr;
-	}
-
-	MS5611 *dev = new MS5611(interface, prom_buf, config);
-
-	if (dev == nullptr) {
-		delete interface;
-		return nullptr;
-	}
-
-	if (OK != dev->init()) {
-		delete dev;
-		return nullptr;
-	}
-
-	return dev;
+        if (config.bus_type == BOARD_SPI_BUS) {
+            interface = MS5611_spi_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency, config.spi_mode);
+        }
+
+    if (interface == nullptr) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
+
+    if (interface->init() != OK) {
+        delete interface;
+        PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid);
+        return nullptr;
+    }
+
+    MS5611 *dev = new MS5611(interface, prom_buf, config);
+
+    if (dev == nullptr) {
+        delete interface;
+        return nullptr;
+    }
+
+    if (OK != dev->init()) {
+        delete dev;
+        return nullptr;
+    }
+
+    return dev;
 }
 
-void MS5611::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("ms5611", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
-	PRINT_MODULE_USAGE_COMMAND("start");
+void MS5611::print_usage() {
+    PRINT_MODULE_USAGE_NAME("ms5611", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+    PRINT_MODULE_USAGE_COMMAND("start");
 #if defined(CONFIG_I2C)
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
 #else
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
 #endif
-	PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611", "Device type", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611", "Device type", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int ms5611_main(int argc, char *argv[])
-{
-	using ThisDriver = MS5611;
-	int ch;
+extern "C" int ms5611_main(int argc, char *argv[]) {
+    using ThisDriver = MS5611;
+    int ch;
 #if defined(CONFIG_I2C)
-	BusCLIArguments cli {true, true};
-	cli.default_i2c_frequency = 400000;
-	cli.i2c_address = MS5611_ADDRESS_1;
+    BusCLIArguments cli{true, true};
+    cli.default_i2c_frequency = 400000;
+    cli.i2c_address           = MS5611_ADDRESS_1;
 #else
-	BusCLIArguments cli {false, true};
+    BusCLIArguments cli{false, true};
 #endif
-	cli.default_spi_frequency = 16 * 1000 * 1000;
-	uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5611;
+    cli.default_spi_frequency = 16 * 1000 * 1000;
+    uint16_t dev_type_driver  = DRV_BARO_DEVTYPE_MS5611;
 
-	while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) {
-		switch (ch) {
-		case 'T': {
-				int val = atoi(cli.optArg());
+    while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) {
+        switch (ch) {
+        case 'T': {
+            int val = atoi(cli.optArg());
 
-				if (val == 5611) {
-					dev_type_driver = DRV_BARO_DEVTYPE_MS5611;
+            if (val == 5611) {
+                dev_type_driver = DRV_BARO_DEVTYPE_MS5611;
 
-				} else if (val == 5607) {
-					dev_type_driver = DRV_BARO_DEVTYPE_MS5607;
-				}
-			}
-			break;
-		}
-	}
+            } else if (val == 5607) {
+                dev_type_driver = DRV_BARO_DEVTYPE_MS5607;
+            }
+        } break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
+    BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/baro/ms5611/ms5611_spi.cpp b/apps/peripheral/sensor/baro/ms5611/ms5611_spi.cpp
index 0125715afb..9f1acc80f5 100644
--- a/apps/peripheral/sensor/baro/ms5611/ms5611_spi.cpp
+++ b/apps/peripheral/sensor/baro/ms5611/ms5611_spi.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ms5611_spi.cpp
@@ -41,226 +18,207 @@
 #include "ms5611.h"
 
 /* SPI protocol address bits */
-#define DIR_READ			(1<<7)
-#define DIR_WRITE			(0<<7)
-#define ADDR_INCREMENT			(1<<6)
-
+#define DIR_READ       (1 << 7)
+#define DIR_WRITE      (0 << 7)
+#define ADDR_INCREMENT (1 << 6)
 
-class MS5611_SPI : public device::SPI
-{
+class MS5611_SPI : public device::SPI {
 public:
-	MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf, int bus_frequency, spi_mode_e spi_mode);
-	virtual ~MS5611_SPI() = default;
+    MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf, int bus_frequency, spi_mode_e spi_mode);
+    virtual ~MS5611_SPI() = default;
 
-	virtual int	init();
-	virtual int	read(unsigned offset, void *data, unsigned count);
-	virtual int	ioctl(unsigned operation, unsigned &arg);
+    virtual int init();
+    virtual int read(unsigned offset, void *data, unsigned count);
+    virtual int ioctl(unsigned operation, unsigned &arg);
 
 private:
-	ms5611::prom_u	&_prom;
+    ms5611::prom_u &_prom;
 
-	/**
+    /**
 	 * Send a reset command to the MS5611.
 	 *
 	 * This is required after any bus reset.
 	 */
-	int		_reset();
+    int _reset();
 
-	/**
+    /**
 	 * Send a measure command to the MS5611.
 	 *
 	 * @param addr		Which address to use for the measure operation.
 	 */
-	int		_measure(unsigned addr);
+    int _measure(unsigned addr);
 
-	/**
+    /**
 	 * Read the MS5611 PROM
 	 *
 	 * @return		OK if the PROM reads successfully.
 	 */
-	int		_read_prom();
+    int _read_prom();
 
-	/**
+    /**
 	 * Read a 16-bit register value.
 	 *
 	 * @param reg		The register to read.
 	 */
-	uint16_t	_reg16(unsigned reg);
+    uint16_t _reg16(unsigned reg);
 
-	/**
+    /**
 	 * Wrapper around transfer() that prevents interrupt-context transfers
 	 * from pre-empting us. The sensor may (does) share a bus with sensors
 	 * that are polled from interrupt context (or we may be pre-empted)
 	 * so we need to guarantee that transfers complete without interruption.
 	 */
-	int		_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+    int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
 };
 
 device::Device *
-MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency, spi_mode_e spi_mode)
-{
-	return new MS5611_SPI(busnum, devid, prom_buf, bus_frequency, spi_mode);
+MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency, spi_mode_e spi_mode) {
+    return new MS5611_SPI(busnum, devid, prom_buf, bus_frequency, spi_mode);
 }
 
 MS5611_SPI::MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_BARO_DEVTYPE_MS5611, MODULE_NAME, bus, device, spi_mode, bus_frequency),
-	_prom(prom_buf)
-{
+    SPI(DRV_BARO_DEVTYPE_MS5611, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+    _prom(prom_buf) {
 }
 
-int
-MS5611_SPI::init()
-{
-	int ret = SPI::init();
+int MS5611_SPI::init() {
+    int ret = SPI::init();
 
-	if (ret != OK) {
-		PX4_DEBUG("SPI init failed");
-		return PX4_ERROR;
-	}
+    if (ret != OK) {
+        PX4_DEBUG("SPI init failed");
+        return PX4_ERROR;
+    }
 
-	// reset and read PROM (try up to 3 times)
-	for (int i = 0; i < 3; i++) {
-		/* send reset command */
-		ret = _reset();
+    // reset and read PROM (try up to 3 times)
+    for (int i = 0; i < 3; i++) {
+        /* send reset command */
+        ret = _reset();
 
-		if (ret != OK) {
-			PX4_DEBUG("reset failed");
-			continue;
-		}
+        if (ret != OK) {
+            PX4_DEBUG("reset failed");
+            continue;
+        }
 
-		/* read PROM */
-		ret = _read_prom();
+        /* read PROM */
+        ret = _read_prom();
 
-		if (ret == OK) {
-			return PX4_OK;
+        if (ret == OK) {
+            return PX4_OK;
 
-		} else {
-			PX4_DEBUG("prom readout failed");
-			continue;
-		}
-	}
+        } else {
+            PX4_DEBUG("prom readout failed");
+            continue;
+        }
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-int
-MS5611_SPI::read(unsigned offset, void *data, unsigned count)
-{
-	union _cvt {
-		uint8_t	b[4];
-		uint32_t w;
-	} *cvt = (_cvt *)data;
-	uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 };
-
-	/* read the most recent measurement */
-	int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
-
-	if (ret == OK) {
-		/* fetch the raw value */
-		cvt->b[0] = buf[3];
-		cvt->b[1] = buf[2];
-		cvt->b[2] = buf[1];
-		cvt->b[3] = 0;
-
-		ret = count;
-	}
-
-	return ret;
+int MS5611_SPI::read(unsigned offset, void *data, unsigned count) {
+    union _cvt {
+        uint8_t  b[4];
+        uint32_t w;
+    } *cvt = (_cvt *)data;
+
+    uint8_t buf[4] = {0 | DIR_WRITE, 0, 0, 0};
+
+    /* read the most recent measurement */
+    int ret = _transfer(&buf[0], &buf[0], sizeof(buf));
+
+    if (ret == OK) {
+        /* fetch the raw value */
+        cvt->b[0] = buf[3];
+        cvt->b[1] = buf[2];
+        cvt->b[2] = buf[1];
+        cvt->b[3] = 0;
+
+        ret = count;
+    }
+
+    return ret;
 }
 
-int
-MS5611_SPI::ioctl(unsigned operation, unsigned &arg)
-{
-	int ret;
+int MS5611_SPI::ioctl(unsigned operation, unsigned &arg) {
+    int ret;
 
-	switch (operation) {
-	case IOCTL_RESET:
-		ret = _reset();
-		break;
+    switch (operation) {
+    case IOCTL_RESET:
+        ret = _reset();
+        break;
 
-	case IOCTL_MEASURE:
-		ret = _measure(arg);
-		break;
+    case IOCTL_MEASURE:
+        ret = _measure(arg);
+        break;
 
-	default:
-		ret = EINVAL;
-	}
+    default:
+        ret = EINVAL;
+    }
 
-	if (ret != OK) {
-		errno = ret;
-		return -1;
-	}
+    if (ret != OK) {
+        errno = ret;
+        return -1;
+    }
 
-	return 0;
+    return 0;
 }
 
-int
-MS5611_SPI::_reset()
-{
-	uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
+int MS5611_SPI::_reset() {
+    uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE;
 
-	return  _transfer(&cmd, nullptr, 1);
+    return _transfer(&cmd, nullptr, 1);
 }
 
-int
-MS5611_SPI::_measure(unsigned addr)
-{
-	uint8_t cmd = addr | DIR_WRITE;
+int MS5611_SPI::_measure(unsigned addr) {
+    uint8_t cmd = addr | DIR_WRITE;
 
-	return _transfer(&cmd, nullptr, 1);
+    return _transfer(&cmd, nullptr, 1);
 }
 
-
-int
-MS5611_SPI::_read_prom()
-{
-	/*
+int MS5611_SPI::_read_prom() {
+    /*
 	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
 	 * called immediately after reset.
 	 */
-	px4_usleep(3000);
+    px4_usleep(3000);
 
-	/* read and convert PROM words */
-	bool all_zero = true;
+    /* read and convert PROM words */
+    bool all_zero = true;
 
-	for (int i = 0; i < 8; i++) {
-		uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
-		_prom.c[i] = _reg16(cmd);
+    for (int i = 0; i < 8; i++) {
+        uint8_t cmd = (ADDR_PROM_SETUP + (i * 2));
+        _prom.c[i]  = _reg16(cmd);
 
-		if (_prom.c[i] != 0) {
-			all_zero = false;
-		}
+        if (_prom.c[i] != 0) {
+            all_zero = false;
+        }
 
-		//PX4_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
-	}
+        //PX4_DEBUG("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]);
+    }
 
-	/* calculate CRC and return success/failure accordingly */
-	int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
+    /* calculate CRC and return success/failure accordingly */
+    int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO;
 
-	if (ret != OK) {
-		PX4_DEBUG("crc failed");
-	}
+    if (ret != OK) {
+        PX4_DEBUG("crc failed");
+    }
 
-	if (all_zero) {
-		PX4_DEBUG("prom all zero");
-		ret = -EIO;
-	}
+    if (all_zero) {
+        PX4_DEBUG("prom all zero");
+        ret = -EIO;
+    }
 
-	return ret;
+    return ret;
 }
 
 uint16_t
-MS5611_SPI::_reg16(unsigned reg)
-{
-	uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
+MS5611_SPI::_reg16(unsigned reg) {
+    uint8_t cmd[3] = {(uint8_t)(reg | DIR_READ), 0, 0};
 
-	_transfer(cmd, cmd, sizeof(cmd));
+    _transfer(cmd, cmd, sizeof(cmd));
 
-	return (uint16_t)(cmd[1] << 8) | cmd[2];
+    return (uint16_t)(cmd[1] << 8) | cmd[2];
 }
 
-int
-MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
-{
-	return transfer(send, recv, len);
+int MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) {
+    return transfer(send, recv, len);
 }
diff --git a/apps/peripheral/sensor/gnss/gps/definitions.h b/apps/peripheral/sensor/gnss/gps/definitions.h
index a1f5ac6a17..24162e8e4c 100644
--- a/apps/peripheral/sensor/gnss/gps/definitions.h
+++ b/apps/peripheral/sensor/gnss/gps/definitions.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2016 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file definitions.h
@@ -48,9 +25,9 @@
 
 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
 #define GPS_WARN(...) PX4_WARN(__VA_ARGS__)
-#define GPS_ERR(...) PX4_ERR(__VA_ARGS__)
+#define GPS_ERR(...)  PX4_ERR(__VA_ARGS__)
 
-#define gps_usleep px4_usleep
+#define gps_usleep    px4_usleep
 
 /**
  * Get the current time in us. Function signature:
@@ -62,5 +39,5 @@ typedef hrt_abstime gps_abstime;
 
 // TODO: this functionality is not available on the Snapdragon yet
 #ifdef __PX4_QURT
-#define NO_MKTIME
+#   define NO_MKTIME
 #endif
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.cpp
index 4406c8b764..f6f24c49fa 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <stdlib.h>
 #include <stdio.h>
@@ -40,26 +17,26 @@
 #include "ashtech.h"
 #include "rtcm.h"
 
-#define MIN(X,Y)	((X) < (Y) ? (X) : (Y))
+#define MIN(X, Y)     ((X) < (Y) ? (X) : (Y))
 #define ASH_UNUSED(x) (void)x;
 
 //#define ASH_DEBUG(...)		{GPS_WARN(__VA_ARGS__);}
-#define ASH_DEBUG(...)		{/*GPS_WARN(__VA_ARGS__);*/}
+#define ASH_DEBUG(...)           \
+    { /*GPS_WARN(__VA_ARGS__);*/ \
+    }
 
 GPSDriverAshtech::GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user,
-				   sensor_gps_s *gps_position, satellite_info_s *satellite_info,
-				   float heading_offset) :
-	GPSBaseStationSupport(callback, callback_user),
-	_heading_offset(heading_offset),
-	_gps_position(gps_position),
-	_satellite_info(satellite_info)
-{
-	decodeInit();
+                                   sensor_gps_s *gps_position, satellite_info_s *satellite_info,
+                                   float heading_offset) :
+    GPSBaseStationSupport(callback, callback_user),
+    _heading_offset(heading_offset),
+    _gps_position(gps_position),
+    _satellite_info(satellite_info) {
+    decodeInit();
 }
 
-GPSDriverAshtech::~GPSDriverAshtech()
-{
-	delete _rtcm_parsing;
+GPSDriverAshtech::~GPSDriverAshtech() {
+    delete _rtcm_parsing;
 }
 
 /*
@@ -67,25 +44,26 @@ GPSDriverAshtech::~GPSDriverAshtech()
  * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
  */
 
-int GPSDriverAshtech::handleMessage(int len)
-{
-	char *endp;
+int GPSDriverAshtech::handleMessage(int len) {
+    char *endp;
 
-	if (len < 7) {
-		return 0;
-	}
+    if (len < 7) {
+        return 0;
+    }
 
-	int uiCalcComma = 0;
+    int uiCalcComma = 0;
 
-	for (int i = 0 ; i < len; i++) {
-		if (_rx_buffer[i] == ',') { uiCalcComma++; }
-	}
+    for (int i = 0; i < len; i++) {
+        if (_rx_buffer[i] == ',') {
+            uiCalcComma++;
+        }
+    }
 
-	char *bufptr = (char *)(_rx_buffer + 6);
-	int ret = 0;
+    char *bufptr = (char *)(_rx_buffer + 6);
+    int   ret    = 0;
 
-	if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
-		/*
+    if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
+        /*
 		UTC day, month, and year, and local time zone offset
 		An example of the ZDA message string is:
 
@@ -103,72 +81,90 @@ int GPSDriverAshtech::handleMessage(int len)
 		7	The checksum data, always begins with *
 		Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
 		*/
-		double ashtech_time = 0.0;
-		int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
-		ASH_UNUSED(local_time_off_min);
-		ASH_UNUSED(local_time_off_hour);
-
-		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-
-		int ashtech_hour = static_cast<int>(ashtech_time / 10000);
-		int ashtech_minute = static_cast<int>((ashtech_time - ashtech_hour * 10000) / 100);
-		double ashtech_sec = static_cast<double>(ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100);
-
-		/*
+        double ashtech_time = 0.0;
+        int    day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
+        ASH_UNUSED(local_time_off_min);
+        ASH_UNUSED(local_time_off_hour);
+
+        if (bufptr && *(++bufptr) != ',') {
+            ashtech_time = strtod(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            day    = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            month  = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            year   = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            local_time_off_hour = strtol(bufptr, &endp, 10);
+            bufptr              = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            local_time_off_min = strtol(bufptr, &endp, 10);
+            bufptr             = endp;
+        }
+
+
+        int    ashtech_hour   = static_cast<int>(ashtech_time / 10000);
+        int    ashtech_minute = static_cast<int>((ashtech_time - ashtech_hour * 10000) / 100);
+        double ashtech_sec    = static_cast<double>(ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100);
+
+        /*
 		 * convert to unix timestamp
 		 */
-		struct tm timeinfo = {};
-		timeinfo.tm_year = year - 1900;
-		timeinfo.tm_mon = month - 1;
-		timeinfo.tm_mday = day;
-		timeinfo.tm_hour = ashtech_hour;
-		timeinfo.tm_min = ashtech_minute;
-		timeinfo.tm_sec = int(ashtech_sec);
-		timeinfo.tm_isdst = 0;
+        struct tm timeinfo = {};
+        timeinfo.tm_year   = year - 1900;
+        timeinfo.tm_mon    = month - 1;
+        timeinfo.tm_mday   = day;
+        timeinfo.tm_hour   = ashtech_hour;
+        timeinfo.tm_min    = ashtech_minute;
+        timeinfo.tm_sec    = int(ashtech_sec);
+        timeinfo.tm_isdst  = 0;
 
 #ifndef NO_MKTIME
-		time_t epoch = mktime(&timeinfo);
+        time_t epoch = mktime(&timeinfo);
 
-		if (epoch > GPS_EPOCH_SECS) {
-			uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1000000;
+        if (epoch > GPS_EPOCH_SECS) {
+            uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1000000;
 
-			// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-			// and control its drift. Since we rely on the HRT for our monotonic
-			// clock, updating it from time to time is safe.
+            // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+            // and control its drift. Since we rely on the HRT for our monotonic
+            // clock, updating it from time to time is safe.
 
-			timespec ts{};
-			ts.tv_sec = epoch;
-			ts.tv_nsec = usecs * 1000;
+            timespec ts{};
+            ts.tv_sec  = epoch;
+            ts.tv_nsec = usecs * 1000;
 
-			setClock(ts);
+            setClock(ts);
 
-			_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-			_gps_position->time_utc_usec += usecs;
+            _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+            _gps_position->time_utc_usec += usecs;
 
-		} else {
-			_gps_position->time_utc_usec = 0;
-		}
+        } else {
+            _gps_position->time_utc_usec = 0;
+        }
 
 #else
-		_gps_position->time_utc_usec = 0;
+        _gps_position->time_utc_usec = 0;
 #endif
 
-		_last_timestamp_time = gps_absolute_time();
-	}
+        _last_timestamp_time = gps_absolute_time();
+    }
 
-	else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14) && !_got_pashr_pos_message) {
-		/*
+    else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14) && !_got_pashr_pos_message) {
+        /*
 		  Time, position, and fix related data
 		  An example of the GBS message string is:
 
@@ -205,77 +201,104 @@ int GPSDriverAshtech::handleMessage(int len)
 		  The checksum data, always begins with *
 		  Note - If a user-defined geoid model, or an inclined
 		*/
-		double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
-		int num_of_sv = 0, fix_quality = 0;
-		double hdop = 99.9;
-		char ns = '?', ew = '?';
-
-		ASH_UNUSED(ashtech_time);
-		ASH_UNUSED(num_of_sv);
-		ASH_UNUSED(hdop);
-
-		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (ns == 'S') {
-			lat = -lat;
-		}
-
-		if (ew == 'W') {
-			lon = -lon;
-		}
-
-		/* convert from degrees, minutes and seconds to degrees * 1e7 */
-		_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->alt = static_cast<int>(alt * 1000);
-		_rate_count_lat_lon++;
-
-		if (fix_quality <= 0) {
-			_gps_position->fix_type = 0;
-
-		} else {
-			/*
+        double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+        int    num_of_sv = 0, fix_quality = 0;
+        double hdop = 99.9;
+        char   ns = '?', ew = '?';
+
+        ASH_UNUSED(ashtech_time);
+        ASH_UNUSED(num_of_sv);
+        ASH_UNUSED(hdop);
+
+        if (bufptr && *(++bufptr) != ',') {
+            ashtech_time = strtod(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ns = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ew = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            fix_quality = strtol(bufptr, &endp, 10);
+            bufptr      = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            num_of_sv = strtol(bufptr, &endp, 10);
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            hdop   = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            alt    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (ns == 'S') {
+            lat = -lat;
+        }
+
+        if (ew == 'W') {
+            lon = -lon;
+        }
+
+        /* convert from degrees, minutes and seconds to degrees * 1e7 */
+        _gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->alt = static_cast<int>(alt * 1000);
+        _rate_count_lat_lon++;
+
+        if (fix_quality <= 0) {
+            _gps_position->fix_type = 0;
+
+        } else {
+            /*
 			 * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
 			 * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
 			 */
-			if (fix_quality == 5) { fix_quality = 3; }
+            if (fix_quality == 5) {
+                fix_quality = 3;
+            }
 
-			/*
+            /*
 			 * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
 			 */
-			_gps_position->fix_type = 3 + fix_quality - 1;
-		}
-
-		_gps_position->timestamp = gps_absolute_time();
-
-		_gps_position->vel_m_s = 0;                                  /**< GPS ground speed (m/s) */
-		_gps_position->vel_n_m_s = 0;                                /**< GPS ground speed in m/s */
-		_gps_position->vel_e_m_s = 0;                                /**< GPS ground speed in m/s */
-		_gps_position->vel_d_m_s = 0;                                /**< GPS ground speed in m/s */
-		_gps_position->cog_rad =
-			0;                                  /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
-		_gps_position->vel_ned_valid = true;                         /**< Flag to indicate if NED speed is valid */
-		_gps_position->c_variance_rad = 0.1f;
-		ret = 1;
-
-	} else if (memcmp(_rx_buffer, "$GPHDT,", 7) == 0 && uiCalcComma == 2) {
-		/*
+            _gps_position->fix_type = 3 + fix_quality - 1;
+        }
+
+        _gps_position->timestamp = gps_absolute_time();
+
+        _gps_position->vel_m_s   = 0;         /**< GPS ground speed (m/s) */
+        _gps_position->vel_n_m_s = 0;         /**< GPS ground speed in m/s */
+        _gps_position->vel_e_m_s = 0;         /**< GPS ground speed in m/s */
+        _gps_position->vel_d_m_s = 0;         /**< GPS ground speed in m/s */
+        _gps_position->cog_rad =
+            0;                                /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+        _gps_position->vel_ned_valid  = true; /**< Flag to indicate if NED speed is valid */
+        _gps_position->c_variance_rad = 0.1f;
+        ret                           = 1;
+
+    } else if (memcmp(_rx_buffer, "$GPHDT,", 7) == 0 && uiCalcComma == 2) {
+        /*
 		Heading message
 		Example $GPHDT,121.2,T*35
 
@@ -283,26 +306,27 @@ int GPSDriverAshtech::handleMessage(int len)
 		T "T" for "True"
 		 */
 
-		float heading = 0.f;
+        float heading = 0.f;
 
-		if (bufptr && *(++bufptr) != ',') {
-			heading = strtof(bufptr, &endp); bufptr = endp;
+        if (bufptr && *(++bufptr) != ',') {
+            heading = strtof(bufptr, &endp);
+            bufptr  = endp;
 
-			ASH_DEBUG("heading update: %.3f", (double)heading);
+            ASH_DEBUG("heading update: %.3f", (double)heading);
 
-			heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
-			heading -= _heading_offset; // range: [-pi, 3pi]
+            heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
+            heading -= _heading_offset; // range: [-pi, 3pi]
 
-			if (heading > M_PI_F) {
-				heading -= 2.f * M_PI_F; // final range is [-pi, pi]
-			}
+            if (heading > M_PI_F) {
+                heading -= 2.f * M_PI_F; // final range is [-pi, pi]
+            }
 
-			_gps_position->heading = heading;
-		}
+            _gps_position->heading = heading;
+        }
 
-	} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
-		_got_pashr_pos_message = true;
-		/*
+    } else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
+        _got_pashr_pos_message = true;
+        /*
 		Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
 
 		    $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc
@@ -331,140 +355,180 @@ int GPSDriverAshtech::handleMessage(int len)
 		      s17 Reserved no data
 		      *cc Checksum
 		    */
-		bufptr = (char *)(_rx_buffer + 10);
+        bufptr = (char *)(_rx_buffer + 10);
 
-		/*
+        /*
 		 * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
 		 */
-		int coordinatesFound = 0;
-		double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
-		int num_of_sv = 0, fix_quality = 0;
-		double track_true = 0.0, ground_speed = 0.0, age_of_corr = 0.0;
-		double hdop = 99.9, vdop = 99.9,  pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
-		char ns = '?', ew = '?';
-
-		ASH_UNUSED(ashtech_time);
-		ASH_UNUSED(num_of_sv);
-		ASH_UNUSED(age_of_corr);
-		ASH_UNUSED(pdop);
-		ASH_UNUSED(tdop);
-
-		if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') {
-			/*
+        int    coordinatesFound = 0;
+        double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+        int    num_of_sv = 0, fix_quality = 0;
+        double track_true = 0.0, ground_speed = 0.0, age_of_corr = 0.0;
+        double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
+        char   ns = '?', ew = '?';
+
+        ASH_UNUSED(ashtech_time);
+        ASH_UNUSED(num_of_sv);
+        ASH_UNUSED(age_of_corr);
+        ASH_UNUSED(pdop);
+        ASH_UNUSED(tdop);
+
+        if (bufptr && *(++bufptr) != ',') {
+            fix_quality = strtol(bufptr, &endp, 10);
+            bufptr      = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            num_of_sv = strtol(bufptr, &endp, 10);
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ashtech_time = strtod(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            /*
 			 * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
 			 * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
 			 */
-			lat = strtod(bufptr, &endp);
-
-			if (bufptr != endp) {coordinatesFound++;}
-
-			bufptr = endp;
-		}
-
-		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') {
-			lon = strtod(bufptr, &endp);
-
-			if (bufptr != endp) {coordinatesFound++;}
-
-			bufptr = endp;
-		}
-
-		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') {
-			alt = strtod(bufptr, &endp);
-
-			if (bufptr != endp) {coordinatesFound++;}
-
-			bufptr = endp;
-		}
-
-		if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
+            lat = strtod(bufptr, &endp);
 
-		if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
+            if (bufptr != endp) {
+                coordinatesFound++;
+            }
 
-		if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
+            bufptr = endp;
+        }
 
-		if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
+        if (bufptr && *(++bufptr) != ',') {
+            ns = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon = strtod(bufptr, &endp);
+
+            if (bufptr != endp) {
+                coordinatesFound++;
+            }
 
-		if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
+            bufptr = endp;
+        }
 
-		if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
+        if (bufptr && *(++bufptr) != ',') {
+            ew = *(bufptr++);
+        }
 
-		if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (ns == 'S') {
-			lat = -lat;
-		}
-
-		if (ew == 'W') {
-			lon = -lon;
-		}
-
-		_gps_position->lat = static_cast<int>((static_cast<int>(lat * 0.01) + (lat * 0.01 - static_cast<int>(
-				lat * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->lon = static_cast<int>((static_cast<int>(lon * 0.01) + (lon * 0.01 - static_cast<int>(
-				lon * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->alt = static_cast<int>(alt * 1000);
-		_gps_position->hdop = static_cast<float>(hdop);
-		_gps_position->vdop = static_cast<float>(vdop);
-		_rate_count_lat_lon++;
-
-		if (coordinatesFound < 3) {
-			_gps_position->fix_type = 0;
-
-		} else {
-			if (fix_quality == 9 || fix_quality == 10) { // SBAS differential or BeiDou differential
-				_gps_position->fix_type = 4; // use RTCM differential
-
-			} else if (fix_quality == 12 || fix_quality == 22) { // RTK float or RTK float dithered
-				_gps_position->fix_type = 5;
-
-			} else if (fix_quality == 13 || fix_quality == 23) { // RTK fixed or RTK fixed dithered
-				_gps_position->fix_type = 6;
-
-			} else {
-				_gps_position->fix_type = 3 + fix_quality;
-			}
-
-			// we got a valid position, activate correction output if needed
-			if (_configure_done && _output_mode == OutputMode::RTCM &&
-			    _board == AshtechBoard::trimble_mb_two && !_correction_output_activated) {
-				activateCorrectionOutput();
-			}
-		}
-
-		_gps_position->timestamp = gps_absolute_time();
-
-		float track_rad = static_cast<float>(track_true) * M_PI_F / 180.0f;
-
-		float velocity_ms = static_cast<float>(ground_speed) / 1.9438445f;			/** knots to m/s */
-		float velocity_north = static_cast<float>(velocity_ms) * cosf(track_rad);
-		float velocity_east  = static_cast<float>(velocity_ms) * sinf(track_rad);
-
-		_gps_position->vel_m_s = velocity_ms;				/** GPS ground speed (m/s) */
-		_gps_position->vel_n_m_s = velocity_north;			/** GPS ground speed in m/s */
-		_gps_position->vel_e_m_s = velocity_east;			/** GPS ground speed in m/s */
-		_gps_position->vel_d_m_s = static_cast<float>(-vertic_vel);				/** GPS ground speed in m/s */
-		_gps_position->cog_rad =
-			track_rad;				/** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
-		_gps_position->vel_ned_valid = true;				/** Flag to indicate if NED speed is valid */
-		_gps_position->c_variance_rad = 0.1f;
-		_rate_count_vel++;
-		ret = 1;
-
-	} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
-		/*
+        if (bufptr && *(++bufptr) != ',') {
+            alt = strtod(bufptr, &endp);
+
+            if (bufptr != endp) {
+                coordinatesFound++;
+            }
+
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            age_of_corr = strtod(bufptr, &endp);
+            bufptr      = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            track_true = strtod(bufptr, &endp);
+            bufptr     = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ground_speed = strtod(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            vertic_vel = strtod(bufptr, &endp);
+            bufptr     = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            pdop   = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            hdop   = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            vdop   = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            tdop   = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (ns == 'S') {
+            lat = -lat;
+        }
+
+        if (ew == 'W') {
+            lon = -lon;
+        }
+
+        _gps_position->lat  = static_cast<int>((static_cast<int>(lat * 0.01) + (lat * 0.01 - static_cast<int>(lat * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->lon  = static_cast<int>((static_cast<int>(lon * 0.01) + (lon * 0.01 - static_cast<int>(lon * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->alt  = static_cast<int>(alt * 1000);
+        _gps_position->hdop = static_cast<float>(hdop);
+        _gps_position->vdop = static_cast<float>(vdop);
+        _rate_count_lat_lon++;
+
+        if (coordinatesFound < 3) {
+            _gps_position->fix_type = 0;
+
+        } else {
+            if (fix_quality == 9 || fix_quality == 10) {         // SBAS differential or BeiDou differential
+                _gps_position->fix_type = 4;                     // use RTCM differential
+
+            } else if (fix_quality == 12 || fix_quality == 22) { // RTK float or RTK float dithered
+                _gps_position->fix_type = 5;
+
+            } else if (fix_quality == 13 || fix_quality == 23) { // RTK fixed or RTK fixed dithered
+                _gps_position->fix_type = 6;
+
+            } else {
+                _gps_position->fix_type = 3 + fix_quality;
+            }
+
+            // we got a valid position, activate correction output if needed
+            if (_configure_done && _output_mode == OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two && !_correction_output_activated) {
+                activateCorrectionOutput();
+            }
+        }
+
+        _gps_position->timestamp = gps_absolute_time();
+
+        float track_rad = static_cast<float>(track_true) * M_PI_F / 180.0f;
+
+        float velocity_ms    = static_cast<float>(ground_speed) / 1.9438445f; /** knots to m/s */
+        float velocity_north = static_cast<float>(velocity_ms) * cosf(track_rad);
+        float velocity_east  = static_cast<float>(velocity_ms) * sinf(track_rad);
+
+        _gps_position->vel_m_s   = velocity_ms;                     /** GPS ground speed (m/s) */
+        _gps_position->vel_n_m_s = velocity_north;                  /** GPS ground speed in m/s */
+        _gps_position->vel_e_m_s = velocity_east;                   /** GPS ground speed in m/s */
+        _gps_position->vel_d_m_s = static_cast<float>(-vertic_vel); /** GPS ground speed in m/s */
+        _gps_position->cog_rad =
+            track_rad;                                              /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+        _gps_position->vel_ned_valid  = true;                       /** Flag to indicate if NED speed is valid */
+        _gps_position->c_variance_rad = 0.1f;
+        _rate_count_vel++;
+        ret = 1;
+
+    } else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
+        /*
 		  Position error statistics
 		  An example of the GST message string is:
 
@@ -488,39 +552,63 @@ int GPSDriverAshtech::handleMessage(int len)
 		  8   Height 1 sigma error, in meters
 		  9   The checksum data, always begins with *
 		*/
-		double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
-		double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
-
-		ASH_UNUSED(ashtech_time);
-		ASH_UNUSED(min_err);
-		ASH_UNUSED(maj_err);
-		ASH_UNUSED(deg_from_north);
-		ASH_UNUSED(rms_err);
-
-		if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
-
-		_gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
-					   + static_cast<float>(lon_err) * static_cast<float>(lon_err));
-		_gps_position->epv = static_cast<float>(alt_err);
-
-		_gps_position->s_variance_m_s = 0;
-
-	} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
-		/*
+        double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
+        double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
+
+        ASH_UNUSED(ashtech_time);
+        ASH_UNUSED(min_err);
+        ASH_UNUSED(maj_err);
+        ASH_UNUSED(deg_from_north);
+        ASH_UNUSED(rms_err);
+
+        if (bufptr && *(++bufptr) != ',') {
+            ashtech_time = strtod(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            rms_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            maj_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            min_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            deg_from_north = strtod(bufptr, &endp);
+            bufptr         = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            alt_err = strtod(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        _gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
+                                   + static_cast<float>(lon_err) * static_cast<float>(lon_err));
+        _gps_position->epv = static_cast<float>(alt_err);
+
+        _gps_position->s_variance_m_s = 0;
+
+    } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
+        /*
 		  The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
 
 		  $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
@@ -540,703 +628,729 @@ int GPSDriverAshtech::handleMessage(int len)
 		  16-19   Information about fourth SV, same format as fields 4 through 7
 		  20  The checksum data, always begins with *
 		*/
-		/*
+        /*
 		 * currently process only gps, because do not know what
 		 * Global satellite ID I should use for non GPS sats
 		 */
-		bool bGPS = false;
-
-		if (memcmp(_rx_buffer, "$GP", 3) != 0) {
-			return 0;
-
-		} else {
-			bGPS = true;
-		}
-
-		int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
-		struct gsv_sat {
-			int svid;
-			int elevation;
-			int azimuth;
-			int snr;
-			int prn;
-		} sat[4];
-
-		memset(sat, 0, sizeof(sat));
-
-		if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
-			return 0;
-		}
-
-		if (this_msg_num == 0 && bGPS && _satellite_info) {
-			memset(_satellite_info->svid,      0, sizeof(_satellite_info->svid));
-			memset(_satellite_info->used,      0, sizeof(_satellite_info->used));
-			memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
-			memset(_satellite_info->azimuth,   0, sizeof(_satellite_info->azimuth));
-			memset(_satellite_info->snr,       0, sizeof(_satellite_info->snr));
-			memset(_satellite_info->prn,       0, sizeof(_satellite_info->prn));
-		}
-
-		int end = 4;
-
-		if (this_msg_num == all_msg_num) {
-			end =  tot_sv_visible - (this_msg_num - 1) * 4;
-			_gps_position->satellites_used = tot_sv_visible;
-
-			if (_satellite_info) {
-				_satellite_info->count = MIN(tot_sv_visible, satellite_info_s::SAT_INFO_MAX_SATELLITES);
-				_satellite_info->timestamp = gps_absolute_time();
-				ret = 2;
-			}
-		}
-
-		if (_satellite_info) {
-			for (int y = 0 ; y < end ; y++) {
-				if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				_satellite_info->svid[y + (this_msg_num - 1) * 4]      = sat[y].svid;
-				_satellite_info->used[y + (this_msg_num - 1) * 4]      = (sat[y].snr > 0);
-				_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
-				_satellite_info->azimuth[y + (this_msg_num - 1) * 4]   = sat[y].azimuth;
-				_satellite_info->snr[y + (this_msg_num - 1) * 4]       = sat[y].snr;
-				_satellite_info->prn[y + (this_msg_num - 1) * 4]       = sat[y].prn;
-			}
-		}
-
-	} else if (memcmp(_rx_buffer, "$PASHR,NAK", 10) == 0) {
-		ASH_DEBUG("Nack received");
-
-		if (_command_state == NMEACommandState::waiting) {
-			_command_state = NMEACommandState::nack;
-		}
-
-	} else if (memcmp(_rx_buffer, "$PASHR,ACK", 10) == 0) {
-		ASH_DEBUG("Ack received");
-
-		if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::Acked) {
-			_command_state = NMEACommandState::received;
-		}
-
-	} else if (memcmp(_rx_buffer, "$PASHR,PRT,", 11) == 0 && uiCalcComma == 3) {
-		if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::PRT) {
-			_command_state = NMEACommandState::received;
-			_port = _rx_buffer[11];
-			ASH_DEBUG("Connected port: %c", _port);
-		}
-
-	} else if (memcmp(_rx_buffer, "$PASHR,RID,", 11) == 0) {
-		if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RID) {
-			_command_state = NMEACommandState::received;
-
-			if (memcmp(_rx_buffer + 11, "MB2", 3) == 0) {
-				_board = AshtechBoard::trimble_mb_two;
-
-			} else {
-				_board = AshtechBoard::other;
-			}
-
-			ASH_DEBUG("Connected board: %i", (int)_board);
-		}
-
-	} else if (memcmp(_rx_buffer, "$PASHR,RECEIPT,", 15) == 0) {
-		// this is the response to $PASHS,POS,AVG,100
-		// example: $PASHR,RECEIPT,POS,AVG,STARTED,INTERVAL,100,114502.56,28.12.2011
-		if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RECEIPT) {
-			_command_state = NMEACommandState::received;
-		}
-
-		// when finished we get one of the follwing messages:
-		// - successful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,114642.81,28.12.2011,5542.5178481,N,03739.2954994,E,176.334,OK,CONTINUOUS,100.20*09
-		// - unsuccessful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,124628.01,28.12.2011,ERR
-		char *finished_find = strstr((char *)_rx_buffer, "FINISHED,");
-
-		if (finished_find) {
-			const bool error = strstr((const char *)_rx_buffer, "ERR");
-			_survey_in_start = 0;
-
-			if (error) {
-				sendSurveyInStatusUpdate(false, false);
-
-			} else {
-				// extract the position
-				double lat = 0., lon = 0.;
-				float alt = 0.f;
-				char ns = '?', ew = '?';
-				bufptr = finished_find + 9; // skip over FINISHED,
-				// skip the next 2 arguments
-				bufptr = strstr(bufptr, ",");
-
-				if (bufptr) { bufptr = strstr(bufptr + 1, ","); }
-
-				if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-				if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-				if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
-
-				if (ns == 'S') { lat = -lat; }
-
-				if (ew == 'W') { lon = -lon; }
-
-				lat = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
-				lon = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;
-
-				sendSurveyInStatusUpdate(false, true, lat, lon, alt);
-
-				activateRTCMOutput(true);
-			}
-		}
-
-	}
-
-	if (ret == 1) {
-		_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
-	}
-
-
-	// handle survey-in status update
-	if (_survey_in_start != 0) {
-		const gps_abstime now = gps_absolute_time();
-		uint32_t survey_in_duration = (now - _survey_in_start) / 1000000;
-
-		if (survey_in_duration != _base_settings.settings.survey_in.min_dur) {
-			_base_settings.settings.survey_in.min_dur = survey_in_duration;
-			sendSurveyInStatusUpdate(true, false);
-		}
-	}
-
-	return ret;
+        bool bGPS = false;
+
+        if (memcmp(_rx_buffer, "$GP", 3) != 0) {
+            return 0;
+
+        } else {
+            bGPS = true;
+        }
+
+        int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
+
+        struct gsv_sat {
+            int svid;
+            int elevation;
+            int azimuth;
+            int snr;
+            int prn;
+        } sat[4];
+
+        memset(sat, 0, sizeof(sat));
+
+        if (bufptr && *(++bufptr) != ',') {
+            all_msg_num = strtol(bufptr, &endp, 10);
+            bufptr      = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            this_msg_num = strtol(bufptr, &endp, 10);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            tot_sv_visible = strtol(bufptr, &endp, 10);
+            bufptr         = endp;
+        }
+
+        if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
+            return 0;
+        }
+
+        if (this_msg_num == 0 && bGPS && _satellite_info) {
+            memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
+            memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
+            memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
+            memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
+            memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
+            memset(_satellite_info->prn, 0, sizeof(_satellite_info->prn));
+        }
+
+        int end = 4;
+
+        if (this_msg_num == all_msg_num) {
+            end                            = tot_sv_visible - (this_msg_num - 1) * 4;
+            _gps_position->satellites_used = tot_sv_visible;
+
+            if (_satellite_info) {
+                _satellite_info->count     = MIN(tot_sv_visible, satellite_info_s::SAT_INFO_MAX_SATELLITES);
+                _satellite_info->timestamp = gps_absolute_time();
+                ret                        = 2;
+            }
+        }
+
+        if (_satellite_info) {
+            for (int y = 0; y < end; y++) {
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].svid = strtol(bufptr, &endp, 10);
+                    bufptr      = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].elevation = strtol(bufptr, &endp, 10);
+                    bufptr           = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].azimuth = strtol(bufptr, &endp, 10);
+                    bufptr         = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].snr = strtol(bufptr, &endp, 10);
+                    bufptr     = endp;
+                }
+
+                _satellite_info->svid[y + (this_msg_num - 1) * 4]      = sat[y].svid;
+                _satellite_info->used[y + (this_msg_num - 1) * 4]      = (sat[y].snr > 0);
+                _satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
+                _satellite_info->azimuth[y + (this_msg_num - 1) * 4]   = sat[y].azimuth;
+                _satellite_info->snr[y + (this_msg_num - 1) * 4]       = sat[y].snr;
+                _satellite_info->prn[y + (this_msg_num - 1) * 4]       = sat[y].prn;
+            }
+        }
+
+    } else if (memcmp(_rx_buffer, "$PASHR,NAK", 10) == 0) {
+        ASH_DEBUG("Nack received");
+
+        if (_command_state == NMEACommandState::waiting) {
+            _command_state = NMEACommandState::nack;
+        }
+
+    } else if (memcmp(_rx_buffer, "$PASHR,ACK", 10) == 0) {
+        ASH_DEBUG("Ack received");
+
+        if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::Acked) {
+            _command_state = NMEACommandState::received;
+        }
+
+    } else if (memcmp(_rx_buffer, "$PASHR,PRT,", 11) == 0 && uiCalcComma == 3) {
+        if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::PRT) {
+            _command_state = NMEACommandState::received;
+            _port          = _rx_buffer[11];
+            ASH_DEBUG("Connected port: %c", _port);
+        }
+
+    } else if (memcmp(_rx_buffer, "$PASHR,RID,", 11) == 0) {
+        if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RID) {
+            _command_state = NMEACommandState::received;
+
+            if (memcmp(_rx_buffer + 11, "MB2", 3) == 0) {
+                _board = AshtechBoard::trimble_mb_two;
+
+            } else {
+                _board = AshtechBoard::other;
+            }
+
+            ASH_DEBUG("Connected board: %i", (int)_board);
+        }
+
+    } else if (memcmp(_rx_buffer, "$PASHR,RECEIPT,", 15) == 0) {
+        // this is the response to $PASHS,POS,AVG,100
+        // example: $PASHR,RECEIPT,POS,AVG,STARTED,INTERVAL,100,114502.56,28.12.2011
+        if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RECEIPT) {
+            _command_state = NMEACommandState::received;
+        }
+
+        // when finished we get one of the follwing messages:
+        // - successful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,114642.81,28.12.2011,5542.5178481,N,03739.2954994,E,176.334,OK,CONTINUOUS,100.20*09
+        // - unsuccessful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,124628.01,28.12.2011,ERR
+        char *finished_find = strstr((char *)_rx_buffer, "FINISHED,");
+
+        if (finished_find) {
+            const bool error = strstr((const char *)_rx_buffer, "ERR");
+            _survey_in_start = 0;
+
+            if (error) {
+                sendSurveyInStatusUpdate(false, false);
+
+            } else {
+                // extract the position
+                double lat = 0., lon = 0.;
+                float  alt = 0.f;
+                char   ns = '?', ew = '?';
+                bufptr = finished_find + 9; // skip over FINISHED,
+                // skip the next 2 arguments
+                bufptr = strstr(bufptr, ",");
+
+                if (bufptr) {
+                    bufptr = strstr(bufptr + 1, ",");
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    lat    = strtod(bufptr, &endp);
+                    bufptr = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    ns = *(bufptr++);
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    lon    = strtod(bufptr, &endp);
+                    bufptr = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    ew = *(bufptr++);
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    alt    = strtod(bufptr, &endp);
+                    bufptr = endp;
+                }
+
+                if (ns == 'S') {
+                    lat = -lat;
+                }
+
+                if (ew == 'W') {
+                    lon = -lon;
+                }
+
+                lat = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
+                lon = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;
+
+                sendSurveyInStatusUpdate(false, true, lat, lon, alt);
+
+                activateRTCMOutput(true);
+            }
+        }
+    }
+
+    if (ret == 1) {
+        _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
+    }
+
+
+    // handle survey-in status update
+    if (_survey_in_start != 0) {
+        const gps_abstime now                = gps_absolute_time();
+        uint32_t          survey_in_duration = (now - _survey_in_start) / 1000000;
+
+        if (survey_in_duration != _base_settings.settings.survey_in.min_dur) {
+            _base_settings.settings.survey_in.min_dur = survey_in_duration;
+            sendSurveyInStatusUpdate(true, false);
+        }
+    }
+
+    return ret;
 }
 
-void GPSDriverAshtech::activateRTCMOutput(bool reduce_update_rate)
-{
-	char buffer[40];
-	const char *rtcm_options[] = {
-		"$PASHS,NME,POS,%c,ON,0.2\r\n",  // reduce position updates to 5 Hz
-
-		"$PASHS,RT3,1074,%c,ON,1\r\n", // GPS observations
-		"$PASHS,RT3,1084,%c,ON,1\r\n", // GLONASS observations
-		"$PASHS,RT3,1094,%c,ON,1\r\n", // Galileo observations
-
-		"$PASHS,RT3,1114,%c,ON,1\r\n", // QZSS observations
-		"$PASHS,RT3,1124,%c,ON,1\r\n", // BDS observations
-		"$PASHS,RT3,1006,%c,ON,1\r\n", // Static position
-		"$PASHS,RT3,1033,%c,ON,31\r\n", // Antenna and receiver name
-		"$PASHS,RT3,1013,%c,ON,1\r\n", // System parameters
-		"$PASHS,RT3,1029,%c,ON,1\r\n", // ASCII message
-		"$PASHS,RT3,1230,%c,ON\r\n", // GLONASS code phase bias
-
-		// TODO: are these required (these are the ones from u-blox)?
-		"$PASHS,RT3,1005,%c,ON,1\r\n",
-		"$PASHS,RT3,1077,%c,ON,1\r\n",
-		"$PASHS,RT3,1087,%c,ON,1\r\n",
-	};
-
-	unsigned first_index = reduce_update_rate ? 0 : 1;
-
-	for (unsigned int conf_i = first_index; conf_i < sizeof(rtcm_options) / sizeof(rtcm_options[0]); conf_i++) {
-		int str_len = snprintf(buffer, sizeof(buffer), rtcm_options[conf_i], _port);
-
-		if (writeAckedCommand(buffer, str_len, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", buffer);
-		}
-	}
+void GPSDriverAshtech::activateRTCMOutput(bool reduce_update_rate) {
+    char        buffer[40];
+    const char *rtcm_options[] = {
+        "$PASHS,NME,POS,%c,ON,0.2\r\n", // reduce position updates to 5 Hz
+
+        "$PASHS,RT3,1074,%c,ON,1\r\n",  // GPS observations
+        "$PASHS,RT3,1084,%c,ON,1\r\n",  // GLONASS observations
+        "$PASHS,RT3,1094,%c,ON,1\r\n",  // Galileo observations
+
+        "$PASHS,RT3,1114,%c,ON,1\r\n",  // QZSS observations
+        "$PASHS,RT3,1124,%c,ON,1\r\n",  // BDS observations
+        "$PASHS,RT3,1006,%c,ON,1\r\n",  // Static position
+        "$PASHS,RT3,1033,%c,ON,31\r\n", // Antenna and receiver name
+        "$PASHS,RT3,1013,%c,ON,1\r\n",  // System parameters
+        "$PASHS,RT3,1029,%c,ON,1\r\n",  // ASCII message
+        "$PASHS,RT3,1230,%c,ON\r\n",    // GLONASS code phase bias
+
+        // TODO: are these required (these are the ones from u-blox)?
+        "$PASHS,RT3,1005,%c,ON,1\r\n",
+        "$PASHS,RT3,1077,%c,ON,1\r\n",
+        "$PASHS,RT3,1087,%c,ON,1\r\n",
+    };
+
+    unsigned first_index = reduce_update_rate ? 0 : 1;
+
+    for (unsigned int conf_i = first_index; conf_i < sizeof(rtcm_options) / sizeof(rtcm_options[0]); conf_i++) {
+        int str_len = snprintf(buffer, sizeof(buffer), rtcm_options[conf_i], _port);
+
+        if (writeAckedCommand(buffer, str_len, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", buffer);
+        }
+    }
 }
 
-void GPSDriverAshtech::receiveWait(unsigned timeout_min)
-{
-	gps_abstime time_started = gps_absolute_time();
+void GPSDriverAshtech::receiveWait(unsigned timeout_min) {
+    gps_abstime time_started = gps_absolute_time();
 
-	while (gps_absolute_time() < time_started + timeout_min * 1000) {
-		receive(timeout_min);
-	}
+    while (gps_absolute_time() < time_started + timeout_min * 1000) {
+        receive(timeout_min);
+    }
 }
 
-int GPSDriverAshtech::receive(unsigned timeout)
-{
-	{
-
-		uint8_t buf[GPS_READ_BUFFER_SIZE];
+int GPSDriverAshtech::receive(unsigned timeout) {
+    {
+        uint8_t buf[GPS_READ_BUFFER_SIZE];
 
-		/* timeout additional to poll */
-		uint64_t time_started = gps_absolute_time();
+        /* timeout additional to poll */
+        uint64_t time_started = gps_absolute_time();
 
-		int j = 0;
-		int bytes_count = 0;
+        int j           = 0;
+        int bytes_count = 0;
 
-		while (true) {
+        while (true) {
+            /* pass received bytes to the packet decoder */
+            while (j < bytes_count) {
+                int l = 0;
 
-			/* pass received bytes to the packet decoder */
-			while (j < bytes_count) {
-				int l = 0;
-
-				if ((l = parseChar(buf[j])) > 0) {
-					/* return to configure during configuration or to the gps driver during normal work
+                if ((l = parseChar(buf[j])) > 0) {
+                    /* return to configure during configuration or to the gps driver during normal work
 					 * if a packet has arrived */
-					int ret = handleMessage(l);
+                    int ret = handleMessage(l);
 
-					if (ret > 0) {
-						return ret;
-					}
-				}
+                    if (ret > 0) {
+                        return ret;
+                    }
+                }
 
-				j++;
-			}
+                j++;
+            }
 
-			/* everything is read */
-			j = bytes_count = 0;
+            /* everything is read */
+            j = bytes_count = 0;
 
-			/* then poll or read for new data */
-			int ret = read(buf, sizeof(buf), timeout * 2);
+            /* then poll or read for new data */
+            int ret = read(buf, sizeof(buf), timeout * 2);
 
-			if (ret < 0) {
-				/* something went wrong when polling */
-				return -1;
+            if (ret < 0) {
+                /* something went wrong when polling */
+                return -1;
 
-			} else if (ret == 0) {
-				/* Timeout while polling or just nothing read if reading, let's
+            } else if (ret == 0) {
+                /* Timeout while polling or just nothing read if reading, let's
 				 * stay here, and use timeout below. */
 
-			} else if (ret > 0) {
-				/* if we have new data from GPS, go handle it */
-				bytes_count = ret;
-			}
-
-			/* in case we get crap from GPS or time out */
-			if (time_started + timeout * 1000 * 2 < gps_absolute_time()) {
-				return -1;
-			}
-		}
-	}
-
+            } else if (ret > 0) {
+                /* if we have new data from GPS, go handle it */
+                bytes_count = ret;
+            }
+
+            /* in case we get crap from GPS or time out */
+            if (time_started + timeout * 1000 * 2 < gps_absolute_time()) {
+                return -1;
+            }
+        }
+    }
 }
-#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
 
-int GPSDriverAshtech::parseChar(uint8_t b)
-{
-	int iRet = 0;
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A' - 0xA)))
 
-	switch (_decode_state) {
-	/* First, look for sync1 */
-	case NMEADecodeState::uninit:
-		if (b == '$') {
-			_decode_state = NMEADecodeState::got_sync1;
-			_rx_buffer_bytes = 0;
-			_rx_buffer[_rx_buffer_bytes++] = b;
+int GPSDriverAshtech::parseChar(uint8_t b) {
+    int iRet = 0;
 
-		} else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
-			_decode_state = NMEADecodeState::decode_rtcm3;
-			_rtcm_parsing->addByte(b);
+    switch (_decode_state) {
+    /* First, look for sync1 */
+    case NMEADecodeState::uninit:
+        if (b == '$') {
+            _decode_state                  = NMEADecodeState::got_sync1;
+            _rx_buffer_bytes               = 0;
+            _rx_buffer[_rx_buffer_bytes++] = b;
 
-		}
+        } else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
+            _decode_state = NMEADecodeState::decode_rtcm3;
+            _rtcm_parsing->addByte(b);
+        }
 
-		break;
+        break;
 
-	case NMEADecodeState::got_sync1:
-		if (b == '$') {
-			_decode_state = NMEADecodeState::got_sync1;
-			_rx_buffer_bytes = 0;
+    case NMEADecodeState::got_sync1:
+        if (b == '$') {
+            _decode_state    = NMEADecodeState::got_sync1;
+            _rx_buffer_bytes = 0;
 
-		} else if (b == '*') {
-			_decode_state = NMEADecodeState::got_asteriks;
-		}
+        } else if (b == '*') {
+            _decode_state = NMEADecodeState::got_asteriks;
+        }
 
-		if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
-			ASH_DEBUG("buffer overflow");
-			_decode_state = NMEADecodeState::uninit;
-			_rx_buffer_bytes = 0;
+        if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
+            ASH_DEBUG("buffer overflow");
+            _decode_state    = NMEADecodeState::uninit;
+            _rx_buffer_bytes = 0;
 
-		} else {
-			_rx_buffer[_rx_buffer_bytes++] = b;
-		}
+        } else {
+            _rx_buffer[_rx_buffer_bytes++] = b;
+        }
 
-		break;
+        break;
 
-	case NMEADecodeState::got_asteriks:
-		_rx_buffer[_rx_buffer_bytes++] = b;
-		_decode_state = NMEADecodeState::got_first_cs_byte;
-		break;
+    case NMEADecodeState::got_asteriks:
+        _rx_buffer[_rx_buffer_bytes++] = b;
+        _decode_state                  = NMEADecodeState::got_first_cs_byte;
+        break;
 
-	case NMEADecodeState::got_first_cs_byte: {
-			_rx_buffer[_rx_buffer_bytes++] = b;
-			uint8_t checksum = 0;
-			uint8_t *buffer = _rx_buffer + 1;
-			uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
+    case NMEADecodeState::got_first_cs_byte: {
+        _rx_buffer[_rx_buffer_bytes++] = b;
+        uint8_t  checksum              = 0;
+        uint8_t *buffer                = _rx_buffer + 1;
+        uint8_t *bufend                = _rx_buffer + _rx_buffer_bytes - 3;
 
-			for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
+        for (; buffer < bufend; buffer++) {
+            checksum ^= *buffer;
+        }
 
-			if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
-			    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
-				iRet = _rx_buffer_bytes;
-			}
+        if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
+            iRet = _rx_buffer_bytes;
+        }
 
-			decodeInit();
-		}
-		break;
+        decodeInit();
+    } break;
 
-	case NMEADecodeState::decode_rtcm3:
-		if (_rtcm_parsing->addByte(b)) {
-			ASH_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
-			gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
-			decodeInit();
-		}
+    case NMEADecodeState::decode_rtcm3:
+        if (_rtcm_parsing->addByte(b)) {
+            ASH_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
+            gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
+            decodeInit();
+        }
 
-		break;
-	}
+        break;
+    }
 
-	return iRet;
+    return iRet;
 }
 
-void GPSDriverAshtech::decodeInit()
-{
-	_rx_buffer_bytes = 0;
-	_decode_state = NMEADecodeState::uninit;
+void GPSDriverAshtech::decodeInit() {
+    _rx_buffer_bytes = 0;
+    _decode_state    = NMEADecodeState::uninit;
 
-	if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
-		if (!_rtcm_parsing) {
-			_rtcm_parsing = new RTCMParsing();
-		}
+    if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
+        if (!_rtcm_parsing) {
+            _rtcm_parsing = new RTCMParsing();
+        }
 
-		if (_rtcm_parsing) {
-			_rtcm_parsing->reset();
-		}
-	}
+        if (_rtcm_parsing) {
+            _rtcm_parsing->reset();
+        }
+    }
 }
 
-int GPSDriverAshtech::writeAckedCommand(const void *buf, int buf_length, unsigned timeout)
-{
-	if (write(buf, buf_length) != buf_length) {
-		return -1;
-	}
+int GPSDriverAshtech::writeAckedCommand(const void *buf, int buf_length, unsigned timeout) {
+    if (write(buf, buf_length) != buf_length) {
+        return -1;
+    }
 
-	return waitForReply(NMEACommand::Acked, timeout);
+    return waitForReply(NMEACommand::Acked, timeout);
 }
 
-int GPSDriverAshtech::waitForReply(NMEACommand command, const unsigned timeout)
-{
-	gps_abstime time_started = gps_absolute_time();
+int GPSDriverAshtech::waitForReply(NMEACommand command, const unsigned timeout) {
+    gps_abstime time_started = gps_absolute_time();
 
-	ASH_DEBUG("waiting for reply for command %i", (int)command);
+    ASH_DEBUG("waiting for reply for command %i", (int)command);
 
-	_command_state = NMEACommandState::waiting;
-	_waiting_for_command = command;
+    _command_state       = NMEACommandState::waiting;
+    _waiting_for_command = command;
 
-	while (_command_state == NMEACommandState::waiting && gps_absolute_time() < time_started + timeout * 1000) {
-		receive(timeout);
-	}
+    while (_command_state == NMEACommandState::waiting && gps_absolute_time() < time_started + timeout * 1000) {
+        receive(timeout);
+    }
 
-	return _command_state == NMEACommandState::received ? 0 : -1;
+    return _command_state == NMEACommandState::received ? 0 : -1;
 }
 
-int GPSDriverAshtech::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	_output_mode = config.output_mode;
-	_correction_output_activated = false;
-	_configure_done = false;
+int GPSDriverAshtech::configure(unsigned &baudrate, const GPSConfig &config) {
+    _output_mode                 = config.output_mode;
+    _correction_output_activated = false;
+    _configure_done              = false;
 
-	/* Try different baudrates (115200 is the default for Trimble) and request the baudrate that we want.
+    /* Try different baudrates (115200 is the default for Trimble) and request the baudrate that we want.
 	 *
 	 * These are Ashtech proprietary commands, we can use them for auto-detection:
 	 * $PASHS for setting
 	 * $PASHQ for querying
 	 * $PASHR for a response
 	 */
-	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
-	bool success = false;
-
-	unsigned test_baudrate;
-
-	for (unsigned int baud_i = 0; !success && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
-		test_baudrate = baudrates_to_try[baud_i];
-
-		if (baudrate > 0 && baudrate != test_baudrate) {
-			continue; // skip to next baudrate
-		}
-
-		setBaudrate(test_baudrate);
-
-		ASH_DEBUG("baudrate set to %i", test_baudrate);
-
-		const char port_config[] = "$PASHQ,PRT\r\n";  // ask for the current port configuration
-
-		for (int run = 0; run < 2; ++run) { // try several times
-			write(port_config, sizeof(port_config) - 1);
-
-			if (waitForReply(NMEACommand::PRT, ASH_RESPONSE_TIMEOUT) == 0) {
-				ASH_DEBUG("got port for baudrate %i", test_baudrate);
-				success = true;
-				break;
-			}
-		}
-	}
-
-	if (!success) {
-		return -1;
-	}
-
-	// We successfully got a response and know to which port we are connected. Now set the desired baudrate
-	// if it's different from the current one.
-	const unsigned desired_baudrate = 115200; // changing this requires also changing the SPD command
-
-	baudrate = test_baudrate;
-
-	if (baudrate != desired_baudrate) {
-		baudrate = desired_baudrate;
-		const char baud_config[] = "$PASHS,SPD,%c,9\r\n"; // configure baudrate to 115200
-		char baud_config_str[sizeof(baud_config)];
-		int len = snprintf(baud_config_str, sizeof(baud_config_str), baud_config, _port);
-		write(baud_config_str, len);
-		decodeInit();
-		receiveWait(200);
-		decodeInit();
-		setBaudrate(baudrate);
-
-		success = false;
-
-		for (int run = 0; run < 10; ++run) {
-			// We ask for the port config again. If we get a reply, we know that the changed settings work.
-			const char port_config[] = "$PASHQ,PRT\r\n";
-			write(port_config, sizeof(port_config) - 1);
-
-			if (waitForReply(NMEACommand::PRT, ASH_RESPONSE_TIMEOUT) == 0) {
-				success = true;
-				break;
-			}
-		}
-
-		if (!success) {
-			return -1;
-		}
-
-		ASH_DEBUG("Successfully configured the baudrate");
-	}
-
-
-// Additional commands that might be useful:
-//		Reading firmware version:
-//			$PASHQ,VER
-//		Reading installed firmware options:
-//			$PASHQ,OPTION
-//		The output for the Trimble MB-two is:
-//			$PASHR,OPTION,0,SERIAL NUMBER,5730C00370*3E
-//			$PASHR,OPTION,@1,GEOFENCING_WW,034017C7114ED*36
-//			$PASHR,OPTION,N,GPS,0340173F8924D*66
-//			$PASHR,OPTION,G,GLONASS,0340178A9E138*69
-//			$PASHR,OPTION,B,BEIDOU,03401434EC35A*4D
-//			$PASHR,OPTION,X,L1TRACKING,0340119C547B8*40
-//			$PASHR,OPTION,Y,L2TRACKING,034012CD03607*42
-//			$PASHR,OPTION,W,20HZ,034016B5A5225*2A
-//			$PASHR,OPTION,J,RTKROVER,034010C800693*41
-//			$PASHR,OPTION,K,RTKBASE,03401065AB099*7E
-//			$PASHR,OPTION,D,DUO,0340138851415*70
-//			$PASHR,OPTION,S,L3TRACKING,034011C7AB73D*48
-//		Reset the full configuration (however it will lead to a reboot and requires about 15s waiting time)
-//			$PASHS,RST
-
-	// get the board identification
-	const char board_identification[] = "$PASHQ,RID\r\n";
-
-	if (write(board_identification, sizeof(board_identification) - 1) == sizeof(board_identification) - 1) {
-		if (waitForReply(NMEACommand::RID, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", board_identification);
-			return -1;
-		}
-	}
-
-	// Now configure the messages we want
-
-	const char update_rate[] = "$PASHS,POP,20\r\n"; // set internal update rate to 20 Hz
-
-	if (writeAckedCommand(update_rate, sizeof(update_rate) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
-		ASH_DEBUG("command %s failed", update_rate);
-		// for some reason we don't get a response here
-	}
-
-	// Enable dual antenna mode (2: both antennas are L1/L2 GNSS capable, flex mode, avoids the need to determine
-	// the baseline length through a prior calibration stage)
-	// Needs to be set before other commands
-	const bool use_dual_mode = _output_mode != OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two;
-
-	if (use_dual_mode) {
-		ASH_DEBUG("Enabling DUO mode");
-		const char duo_mode[] = "$PASHS,SNS,DUO,2\r\n";
-
-		if (writeAckedCommand(duo_mode, sizeof(duo_mode) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", duo_mode);
-		}
-
-	} else {
-		const char solo_mode[] = "$PASHS,SNS,SOL\r\n";
-
-		if (writeAckedCommand(solo_mode, sizeof(solo_mode) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", solo_mode);
-		}
-	}
-
-	char buffer[40];
-	const char *config_options[] = {
-		"$PASHS,NME,ALL,%c,OFF\r\n",    // disable all NMEA and NMEA-Like Messages
-		"$PASHS,ATM,ALL,%c,OFF\r\n",    // disable all ATM (ATOM) Messages
-		"$PASHS,OUT,%c,ON\r\n",         // enable periodic output
-		"$PASHS,NME,ZDA,%c,ON,3\r\n",   // enable ZDA (date & time) output every 3s
-		"$PASHS,NME,GST,%c,ON,3\r\n",   // position accuracy messages
-		"$PASHS,NME,POS,%c,ON,0.05\r\n",// position & velocity (we can go up to 20Hz if FW option [W] is given and to 50Hz if [8] is given)
-		"$PASHS,NME,GSV,%c,ON,1\r\n"    // satellite status
-	};
-
-	for (unsigned int conf_i = 0; conf_i < sizeof(config_options) / sizeof(config_options[0]); conf_i++) {
-		int len = snprintf(buffer, sizeof(buffer), config_options[conf_i], _port);
-
-		if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", buffer);
-			// some commands are not acked (e.g. GSV), so don't make this fatal
-		}
-	}
-
-	if (use_dual_mode) {
-		// enable heading output
-		const char heading_output[] = "$PASHS,NME,HDT,%c,ON,0.05\r\n";
-		int len = snprintf(buffer, sizeof(buffer), heading_output, _port);
-
-		if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", buffer);
-		}
-	}
-
-
-	if (_output_mode == OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two) {
-		SurveyInStatus status{};
-		status.latitude = status.longitude = (double)NAN;
-		status.altitude = NAN;
-		status.duration = 0;
-		status.mean_accuracy = 0;
-		const bool valid = false;
-		const bool active = true;
-		status.flags = (int)valid | ((int)active << 1);
-		surveyInStatus(status);
-	}
-
-	if (_output_mode == OutputMode::GPSAndRTCM && _board == AshtechBoard::trimble_mb_two) {
-		activateRTCMOutput(false);
-	}
-
-	_configure_done = true;
-	return 0;
+    const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+    bool           success            = false;
+
+    unsigned test_baudrate;
+
+    for (unsigned int baud_i = 0; !success && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
+        test_baudrate = baudrates_to_try[baud_i];
+
+        if (baudrate > 0 && baudrate != test_baudrate) {
+            continue; // skip to next baudrate
+        }
+
+        setBaudrate(test_baudrate);
+
+        ASH_DEBUG("baudrate set to %i", test_baudrate);
+
+        const char port_config[] = "$PASHQ,PRT\r\n"; // ask for the current port configuration
+
+        for (int run = 0; run < 2; ++run) {          // try several times
+            write(port_config, sizeof(port_config) - 1);
+
+            if (waitForReply(NMEACommand::PRT, ASH_RESPONSE_TIMEOUT) == 0) {
+                ASH_DEBUG("got port for baudrate %i", test_baudrate);
+                success = true;
+                break;
+            }
+        }
+    }
+
+    if (!success) {
+        return -1;
+    }
+
+    // We successfully got a response and know to which port we are connected. Now set the desired baudrate
+    // if it's different from the current one.
+    const unsigned desired_baudrate = 115200; // changing this requires also changing the SPD command
+
+    baudrate = test_baudrate;
+
+    if (baudrate != desired_baudrate) {
+        baudrate                 = desired_baudrate;
+        const char baud_config[] = "$PASHS,SPD,%c,9\r\n"; // configure baudrate to 115200
+        char       baud_config_str[sizeof(baud_config)];
+        int        len = snprintf(baud_config_str, sizeof(baud_config_str), baud_config, _port);
+        write(baud_config_str, len);
+        decodeInit();
+        receiveWait(200);
+        decodeInit();
+        setBaudrate(baudrate);
+
+        success = false;
+
+        for (int run = 0; run < 10; ++run) {
+            // We ask for the port config again. If we get a reply, we know that the changed settings work.
+            const char port_config[] = "$PASHQ,PRT\r\n";
+            write(port_config, sizeof(port_config) - 1);
+
+            if (waitForReply(NMEACommand::PRT, ASH_RESPONSE_TIMEOUT) == 0) {
+                success = true;
+                break;
+            }
+        }
+
+        if (!success) {
+            return -1;
+        }
+
+        ASH_DEBUG("Successfully configured the baudrate");
+    }
+
+
+    // Additional commands that might be useful:
+    //		Reading firmware version:
+    //			$PASHQ,VER
+    //		Reading installed firmware options:
+    //			$PASHQ,OPTION
+    //		The output for the Trimble MB-two is:
+    //			$PASHR,OPTION,0,SERIAL NUMBER,5730C00370*3E
+    //			$PASHR,OPTION,@1,GEOFENCING_WW,034017C7114ED*36
+    //			$PASHR,OPTION,N,GPS,0340173F8924D*66
+    //			$PASHR,OPTION,G,GLONASS,0340178A9E138*69
+    //			$PASHR,OPTION,B,BEIDOU,03401434EC35A*4D
+    //			$PASHR,OPTION,X,L1TRACKING,0340119C547B8*40
+    //			$PASHR,OPTION,Y,L2TRACKING,034012CD03607*42
+    //			$PASHR,OPTION,W,20HZ,034016B5A5225*2A
+    //			$PASHR,OPTION,J,RTKROVER,034010C800693*41
+    //			$PASHR,OPTION,K,RTKBASE,03401065AB099*7E
+    //			$PASHR,OPTION,D,DUO,0340138851415*70
+    //			$PASHR,OPTION,S,L3TRACKING,034011C7AB73D*48
+    //		Reset the full configuration (however it will lead to a reboot and requires about 15s waiting time)
+    //			$PASHS,RST
+
+    // get the board identification
+    const char board_identification[] = "$PASHQ,RID\r\n";
+
+    if (write(board_identification, sizeof(board_identification) - 1) == sizeof(board_identification) - 1) {
+        if (waitForReply(NMEACommand::RID, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", board_identification);
+            return -1;
+        }
+    }
+
+    // Now configure the messages we want
+
+    const char update_rate[] = "$PASHS,POP,20\r\n"; // set internal update rate to 20 Hz
+
+    if (writeAckedCommand(update_rate, sizeof(update_rate) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
+        ASH_DEBUG("command %s failed", update_rate);
+        // for some reason we don't get a response here
+    }
+
+    // Enable dual antenna mode (2: both antennas are L1/L2 GNSS capable, flex mode, avoids the need to determine
+    // the baseline length through a prior calibration stage)
+    // Needs to be set before other commands
+    const bool use_dual_mode = _output_mode != OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two;
+
+    if (use_dual_mode) {
+        ASH_DEBUG("Enabling DUO mode");
+        const char duo_mode[] = "$PASHS,SNS,DUO,2\r\n";
+
+        if (writeAckedCommand(duo_mode, sizeof(duo_mode) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", duo_mode);
+        }
+
+    } else {
+        const char solo_mode[] = "$PASHS,SNS,SOL\r\n";
+
+        if (writeAckedCommand(solo_mode, sizeof(solo_mode) - 1, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", solo_mode);
+        }
+    }
+
+    char        buffer[40];
+    const char *config_options[] = {
+        "$PASHS,NME,ALL,%c,OFF\r\n",     // disable all NMEA and NMEA-Like Messages
+        "$PASHS,ATM,ALL,%c,OFF\r\n",     // disable all ATM (ATOM) Messages
+        "$PASHS,OUT,%c,ON\r\n",          // enable periodic output
+        "$PASHS,NME,ZDA,%c,ON,3\r\n",    // enable ZDA (date & time) output every 3s
+        "$PASHS,NME,GST,%c,ON,3\r\n",    // position accuracy messages
+        "$PASHS,NME,POS,%c,ON,0.05\r\n", // position & velocity (we can go up to 20Hz if FW option [W] is given and to 50Hz if [8] is given)
+        "$PASHS,NME,GSV,%c,ON,1\r\n"     // satellite status
+    };
+
+    for (unsigned int conf_i = 0; conf_i < sizeof(config_options) / sizeof(config_options[0]); conf_i++) {
+        int len = snprintf(buffer, sizeof(buffer), config_options[conf_i], _port);
+
+        if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", buffer);
+            // some commands are not acked (e.g. GSV), so don't make this fatal
+        }
+    }
+
+    if (use_dual_mode) {
+        // enable heading output
+        const char heading_output[] = "$PASHS,NME,HDT,%c,ON,0.05\r\n";
+        int        len              = snprintf(buffer, sizeof(buffer), heading_output, _port);
+
+        if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", buffer);
+        }
+    }
+
+
+    if (_output_mode == OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two) {
+        SurveyInStatus status{};
+        status.latitude = status.longitude = (double)NAN;
+        status.altitude                    = NAN;
+        status.duration                    = 0;
+        status.mean_accuracy               = 0;
+        const bool valid                   = false;
+        const bool active                  = true;
+        status.flags                       = (int)valid | ((int)active << 1);
+        surveyInStatus(status);
+    }
+
+    if (_output_mode == OutputMode::GPSAndRTCM && _board == AshtechBoard::trimble_mb_two) {
+        activateRTCMOutput(false);
+    }
+
+    _configure_done = true;
+    return 0;
 }
 
-void GPSDriverAshtech::activateCorrectionOutput()
-{
-	if (_correction_output_activated || _output_mode != OutputMode::RTCM) {
-		return;
-	}
+void GPSDriverAshtech::activateCorrectionOutput() {
+    if (_correction_output_activated || _output_mode != OutputMode::RTCM) {
+        return;
+    }
 
-	_correction_output_activated = true;
-	char buffer[100];
+    _correction_output_activated = true;
+    char buffer[100];
 
-	if (_base_settings.type == BaseSettingsType::survey_in) {
-		ASH_DEBUG("enabling survey-in");
+    if (_base_settings.type == BaseSettingsType::survey_in) {
+        ASH_DEBUG("enabling survey-in");
 
-		// setup the base reference: average the position over N seconds
-		const char avg_pos[] = "$PASHS,POS,AVG,%i\r\n";
-		// alternatively use the current position as reference: "$PASHS,POS,CUR\r\n"
-		int len = snprintf(buffer, sizeof(buffer), avg_pos, (int)_base_settings.settings.survey_in.min_dur);
+        // setup the base reference: average the position over N seconds
+        const char avg_pos[] = "$PASHS,POS,AVG,%i\r\n";
+        // alternatively use the current position as reference: "$PASHS,POS,CUR\r\n"
+        int len = snprintf(buffer, sizeof(buffer), avg_pos, (int)_base_settings.settings.survey_in.min_dur);
 
-		write(buffer, len);
+        write(buffer, len);
 
-		if (waitForReply(NMEACommand::RECEIPT, ASH_RESPONSE_TIMEOUT) != 0) {
-			ASH_DEBUG("command %s failed", buffer);
-		}
+        if (waitForReply(NMEACommand::RECEIPT, ASH_RESPONSE_TIMEOUT) != 0) {
+            ASH_DEBUG("command %s failed", buffer);
+        }
 
 
-		const char *config_options[] = {
-			"$PASHS,ANP,OWN,TRM55971.00\r\n",    // set antenna name (arbitrary)
-			"$PASHS,STI,0001\r\n"         // enter a base ID
-		};
+        const char *config_options[] = {
+            "$PASHS,ANP,OWN,TRM55971.00\r\n", // set antenna name (arbitrary)
+            "$PASHS,STI,0001\r\n"             // enter a base ID
+        };
 
 
-		for (unsigned int conf_i = 0; conf_i < sizeof(config_options) / sizeof(config_options[0]); conf_i++) {
-			if (writeAckedCommand(config_options[conf_i], strlen(config_options[conf_i]), ASH_RESPONSE_TIMEOUT) != 0) {
-				ASH_DEBUG("command %s failed", config_options[conf_i]);
-			}
-		}
+        for (unsigned int conf_i = 0; conf_i < sizeof(config_options) / sizeof(config_options[0]); conf_i++) {
+            if (writeAckedCommand(config_options[conf_i], strlen(config_options[conf_i]), ASH_RESPONSE_TIMEOUT) != 0) {
+                ASH_DEBUG("command %s failed", config_options[conf_i]);
+            }
+        }
 
-		_base_settings.settings.survey_in.min_dur = 0; // use it as counter how long survey-in has been active
-		_survey_in_start = gps_absolute_time();
-		sendSurveyInStatusUpdate(true, false);
+        _base_settings.settings.survey_in.min_dur = 0; // use it as counter how long survey-in has been active
+        _survey_in_start                          = gps_absolute_time();
+        sendSurveyInStatusUpdate(true, false);
 
-	} else {
-		ASH_DEBUG("setting base station position");
+    } else {
+        ASH_DEBUG("setting base station position");
 
-		const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
-		char ns, ew;
-		double latitude = settings.latitude;
+        const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
+        char                         ns, ew;
+        double                       latitude = settings.latitude;
 
-		if (latitude < 0.) {
-			latitude = -latitude;
-			ns = 'S';
+        if (latitude < 0.) {
+            latitude = -latitude;
+            ns       = 'S';
 
-		} else {
-			ns = 'N';
-		}
+        } else {
+            ns = 'N';
+        }
 
-		// convert to ddmm.mmmmmm format
-		latitude = ((int)latitude) * 100. + (latitude - ((int)latitude)) * 60.;
+        // convert to ddmm.mmmmmm format
+        latitude = ((int)latitude) * 100. + (latitude - ((int)latitude)) * 60.;
 
-		double longitude = settings.longitude;
+        double longitude = settings.longitude;
 
-		if (longitude < 0.) {
-			longitude = -longitude;
-			ew = 'W';
+        if (longitude < 0.) {
+            longitude = -longitude;
+            ew        = 'W';
 
-		} else {
-			ew = 'E';
-		}
+        } else {
+            ew = 'E';
+        }
 
-		// convert to ddmm.mmmmmm format
-		longitude = ((int)longitude) * 100. + (longitude - ((int)longitude)) * 60.;
+        // convert to ddmm.mmmmmm format
+        longitude = ((int)longitude) * 100. + (longitude - ((int)longitude)) * 60.;
 
-		int len = snprintf(buffer, sizeof(buffer), "$PASHS,POS,%.8f,%c,%.8f,%c,%.5f,PC1",
-				   latitude, ns, longitude, ew, (double)settings.altitude);
+        int len = snprintf(buffer, sizeof(buffer), "$PASHS,POS,%.8f,%c,%.8f,%c,%.5f,PC1",
+                           latitude, ns, longitude, ew, (double)settings.altitude);
 
-		if (len >= 0 && len < (int)sizeof(buffer)) {
-			if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
-				ASH_DEBUG("command %s failed", buffer);
-			}
+        if (len >= 0 && len < (int)sizeof(buffer)) {
+            if (writeAckedCommand(buffer, len, ASH_RESPONSE_TIMEOUT) != 0) {
+                ASH_DEBUG("command %s failed", buffer);
+            }
 
-		} else {
-			ASH_DEBUG("snprintf failed (buffer too short)");
-		}
+        } else {
+            ASH_DEBUG("snprintf failed (buffer too short)");
+        }
 
-		activateRTCMOutput(true);
-		sendSurveyInStatusUpdate(false, true, settings.latitude, settings.longitude, settings.altitude);
-	}
+        activateRTCMOutput(true);
+        sendSurveyInStatusUpdate(false, true, settings.latitude, settings.longitude, settings.altitude);
+    }
 }
 
-void
-GPSDriverAshtech::sendSurveyInStatusUpdate(bool active, bool valid, double latitude, double longitude, float altitude)
-{
-	SurveyInStatus status{};
-	status.latitude = latitude;
-	status.longitude = longitude;
-	status.altitude = altitude;
-	status.duration = _base_settings.settings.survey_in.min_dur;
-	status.mean_accuracy = 0; // unknown
-	status.flags = (int)valid | ((int)active << 1);
-	surveyInStatus(status);
+void GPSDriverAshtech::sendSurveyInStatusUpdate(bool active, bool valid, double latitude, double longitude, float altitude) {
+    SurveyInStatus status{};
+    status.latitude      = latitude;
+    status.longitude     = longitude;
+    status.altitude      = altitude;
+    status.duration      = _base_settings.settings.survey_in.min_dur;
+    status.mean_accuracy = 0; // unknown
+    status.flags         = (int)valid | ((int)active << 1);
+    surveyInStatus(status);
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.h b/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.h
index 2db2925629..756f9dbbfe 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/ashtech.h
@@ -1,37 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2013. All rights reserved.
- *   Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
- *           Kistanov Alexander <akistanov@gramant.ru>
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /** @file ASHTECH protocol definitions */
 
@@ -46,108 +21,106 @@
 class RTCMParsing;
 
 #define ASHTECH_RECV_BUFFER_SIZE 512
-#define ASH_RESPONSE_TIMEOUT     200    // ms, timeout for waiting for a response
+#define ASH_RESPONSE_TIMEOUT     200 // ms, timeout for waiting for a response
 
-class GPSDriverAshtech : public GPSBaseStationSupport
-{
+class GPSDriverAshtech : public GPSBaseStationSupport {
 public:
-	/**
+    /**
 	 * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement.
 	 */
-	GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position,
-			 satellite_info_s *satellite_info, float heading_offset = 0.f);
+    GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position,
+                     satellite_info_s *satellite_info, float heading_offset = 0.f);
 
-	virtual ~GPSDriverAshtech();
+    virtual ~GPSDriverAshtech();
 
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
-	int receive(unsigned timeout) override;
+    int receive(unsigned timeout) override;
 
 private:
-	enum class AshtechBoard {
-		trimble_mb_two,
-		other
-	};
-
-	enum class NMEACommand {
-		Acked, // Command that returns a (N)Ack
-		PRT,   // port config
-		RID,   // board identification
-		RECEIPT// board identification
-	};
-
-	enum class NMEACommandState {
-		idle,
-		waiting,
-		nack,
-		received
-	};
-
-	enum class NMEADecodeState {
-		uninit,
-		got_sync1,
-		got_asteriks,
-		got_first_cs_byte,
-		decode_rtcm3
-	};
-
-	/**
+    enum class AshtechBoard {
+        trimble_mb_two,
+        other
+    };
+
+    enum class NMEACommand {
+        Acked,  // Command that returns a (N)Ack
+        PRT,    // port config
+        RID,    // board identification
+        RECEIPT // board identification
+    };
+
+    enum class NMEACommandState {
+        idle,
+        waiting,
+        nack,
+        received
+    };
+
+    enum class NMEADecodeState {
+        uninit,
+        got_sync1,
+        got_asteriks,
+        got_first_cs_byte,
+        decode_rtcm3
+    };
+
+    /**
 	 * enable output of correction output
 	 */
-	void activateCorrectionOutput();
+    void activateCorrectionOutput();
 
-	void activateRTCMOutput(bool reduce_update_rate);
+    void activateRTCMOutput(bool reduce_update_rate);
 
-	void decodeInit(void);
+    void decodeInit(void);
 
-	int handleMessage(int len);
+    int handleMessage(int len);
 
-	int parseChar(uint8_t b);
+    int parseChar(uint8_t b);
 
-	/**
+    /**
 	 * receive data for at least the specified amount of time
 	 */
-	void receiveWait(unsigned timeout_min);
+    void receiveWait(unsigned timeout_min);
 
-	void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = static_cast<double>(NAN),
-				      double longitude = static_cast<double>(NAN), float altitude = NAN);
+    void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = static_cast<double>(NAN),
+                                  double longitude = static_cast<double>(NAN), float altitude = NAN);
 
-	/**
+    /**
 	 * Write a command and wait for a (N)Ack
 	 * @return 0 on success, <0 otherwise
 	 */
-	int writeAckedCommand(const void *buf, int buf_length, unsigned timeout);
+    int writeAckedCommand(const void *buf, int buf_length, unsigned timeout);
 
-	int waitForReply(NMEACommand command, const unsigned timeout);
+    int waitForReply(NMEACommand command, const unsigned timeout);
 
-	bool _correction_output_activated{false};
-	bool _configure_done{false};
-	bool _got_pashr_pos_message{false}; /**< If we got a PASHR,POS message, we will ignore GGA messages */
+    bool _correction_output_activated{false};
+    bool _configure_done{false};
+    bool _got_pashr_pos_message{false}; /**< If we got a PASHR,POS message, we will ignore GGA messages */
 
-	char _port{'A'}; /**< port we are connected to (e.g. 'A') */
+    char _port{'A'};                    /**< port we are connected to (e.g. 'A') */
 
-	uint8_t _rx_buffer[ASHTECH_RECV_BUFFER_SIZE];
-	uint16_t _rx_buffer_bytes{};
-	uint64_t _last_timestamp_time{0};
+    uint8_t  _rx_buffer[ASHTECH_RECV_BUFFER_SIZE];
+    uint16_t _rx_buffer_bytes{};
+    uint64_t _last_timestamp_time{0};
 
-	float _heading_offset;
+    float _heading_offset;
 
-	gps_abstime _survey_in_start{0};
+    gps_abstime _survey_in_start{0};
 
-	sensor_gps_s *_gps_position {nullptr};
+    sensor_gps_s *_gps_position{nullptr};
 
-	satellite_info_s *_satellite_info {nullptr};
+    satellite_info_s *_satellite_info{nullptr};
 
-	AshtechBoard _board{AshtechBoard::other}; /**< board we are connected to */
+    AshtechBoard _board{AshtechBoard::other}; /**< board we are connected to */
 
-	NMEACommand _waiting_for_command;
+    NMEACommand _waiting_for_command;
 
-	NMEACommandState _command_state{NMEACommandState::idle};
+    NMEACommandState _command_state{NMEACommandState::idle};
 
-	NMEADecodeState _decode_state{NMEADecodeState::uninit};
+    NMEADecodeState _decode_state{NMEADecodeState::uninit};
 
-	OutputMode _output_mode{OutputMode::GPS};
+    OutputMode _output_mode{OutputMode::GPS};
 
-	RTCMParsing *_rtcm_parsing{nullptr};
+    RTCMParsing *_rtcm_parsing{nullptr};
 };
-
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/base_station.h b/apps/peripheral/sensor/gnss/gps/devices/src/base_station.h
index b34e49f5d5..8fc9b1d4fe 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/base_station.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/base_station.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file base_station.h
@@ -46,28 +23,27 @@
  * @class GPSBaseStationSupport
  * GPS driver base class with Base Station Support
  */
-class GPSBaseStationSupport : public GPSHelper
-{
+class GPSBaseStationSupport : public GPSHelper {
 public:
-	GPSBaseStationSupport(GPSCallbackPtr callback, void *callback_user)
-		: GPSHelper(callback, callback_user) {}
+    GPSBaseStationSupport(GPSCallbackPtr callback, void *callback_user) :
+        GPSHelper(callback, callback_user) {
+    }
 
-	virtual ~GPSBaseStationSupport() = default;
+    virtual ~GPSBaseStationSupport() = default;
 
-	/**
+    /**
 	 * set survey-in specs for RTK base station setup (for finding an accurate base station position
 	 * by averaging the position measurements over time).
 	 * @param survey_in_acc_limit minimum accuracy in 0.1mm
 	 * @param survey_in_min_dur minimum duration in seconds
 	 */
-	void setSurveyInSpecs(uint32_t survey_in_acc_limit, uint32_t survey_in_min_dur)
-	{
-		_base_settings.type = BaseSettingsType::survey_in;
-		_base_settings.settings.survey_in.acc_limit = survey_in_acc_limit;
-		_base_settings.settings.survey_in.min_dur = survey_in_min_dur;
-	}
+    void setSurveyInSpecs(uint32_t survey_in_acc_limit, uint32_t survey_in_min_dur) {
+        _base_settings.type                         = BaseSettingsType::survey_in;
+        _base_settings.settings.survey_in.acc_limit = survey_in_acc_limit;
+        _base_settings.settings.survey_in.min_dur   = survey_in_min_dur;
+    }
 
-	/**
+    /**
 	 * Set a fixed base station position. This can be used if the base position is already known to
 	 * avoid doing a survey-in.
 	 * @param latitude [deg]
@@ -75,38 +51,40 @@ class GPSBaseStationSupport : public GPSHelper
 	 * @param altitude [m]
 	 * @param position_accuracy 3D position accuracy (set to 0 if unknown) [mm]
 	 */
-	void setBasePosition(double latitude, double longitude, float altitude, float position_accuracy)
-	{
-		_base_settings.type = BaseSettingsType::fixed_position;
-		_base_settings.settings.fixed_position.latitude = latitude;
-		_base_settings.settings.fixed_position.longitude = longitude;
-		_base_settings.settings.fixed_position.altitude = altitude;
-		_base_settings.settings.fixed_position.position_accuracy = position_accuracy;
-	}
+    void setBasePosition(double latitude, double longitude, float altitude, float position_accuracy) {
+        _base_settings.type                                      = BaseSettingsType::fixed_position;
+        _base_settings.settings.fixed_position.latitude          = latitude;
+        _base_settings.settings.fixed_position.longitude         = longitude;
+        _base_settings.settings.fixed_position.altitude          = altitude;
+        _base_settings.settings.fixed_position.position_accuracy = position_accuracy;
+    }
 
 protected:
+    enum class BaseSettingsType : uint8_t {
+        survey_in,
+        fixed_position
+    };
 
-	enum class BaseSettingsType : uint8_t {
-		survey_in,
-		fixed_position
-	};
-	struct SurveyInSettings {
-		uint32_t acc_limit;
-		uint32_t min_dur;
-	};
-	struct FixedPositionSettings {
-		double latitude;
-		double longitude;
-		float altitude;
-		float position_accuracy;
-	};
-	struct BaseSettings {
-		BaseSettingsType type;
-		union {
-			SurveyInSettings survey_in;
-			FixedPositionSettings fixed_position;
-		} settings;
-	};
-	BaseSettings _base_settings;
-};
+    struct SurveyInSettings {
+        uint32_t acc_limit;
+        uint32_t min_dur;
+    };
 
+    struct FixedPositionSettings {
+        double latitude;
+        double longitude;
+        float  altitude;
+        float  position_accuracy;
+    };
+
+    struct BaseSettings {
+        BaseSettingsType type;
+
+        union {
+            SurveyInSettings      survey_in;
+            FixedPositionSettings fixed_position;
+        } settings;
+    };
+
+    BaseSettings _base_settings;
+};
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/crc.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/crc.cpp
index ed6200ef34..ff2583c315 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/crc.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/crc.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "crc.h"
 
@@ -43,26 +20,24 @@
 
 static constexpr uint32_t CRC32_POLYNOMIAL = 0xEDB88320;
 
-static uint32_t crc32Value(uint32_t crc)
-{
-	for (int i = 8 ; i > 0; i--) {
-		if (crc & 1) {
-			crc = (crc >> 1) ^ CRC32_POLYNOMIAL;
+static uint32_t crc32Value(uint32_t crc) {
+    for (int i = 8; i > 0; i--) {
+        if (crc & 1) {
+            crc = (crc >> 1) ^ CRC32_POLYNOMIAL;
 
-		} else {
-			crc >>= 1;
-		}
-	}
+        } else {
+            crc >>= 1;
+        }
+    }
 
-	return crc;
+    return crc;
 }
 
 uint32_t
-calculateCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
-{
-	while (length-- != 0) {
-		crc = ((crc >> 8) & 0x00FFFFFFL) ^ (crc32Value(((uint32_t) crc ^ *buffer++) & 0xff));
-	}
+calculateCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) {
+    while (length-- != 0) {
+        crc = ((crc >> 8) & 0x00FFFFFFL) ^ (crc32Value(((uint32_t)crc ^ *buffer++) & 0xff));
+    }
 
-	return crc;
+    return crc;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/crc.h b/apps/peripheral/sensor/gnss/gps/devices/src/crc.h
index dacdb0b7e0..24ad9b3859 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/crc.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/crc.h
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 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.
- *
- ****************************************************************************/
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.cpp
index 75f774a21e..617e5467e3 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file emlid_reach.cpp
@@ -51,326 +28,303 @@
 
 //// ERB
 // 'E' Ox45 | 'R' Ox52 | ID | LENGTH (2 Bytes little endian) | payload | CHECKSUM (2B)
-#define ERB_SYNC_1 0x45 // E
-#define ERB_SYNC_2 0x52 // R
-
-#define ERB_ID_VERSION         0x01
-#define ERB_ID_GEODIC_POSITION 0x02
-#define ERB_ID_NAV_STATUS      0x03
-#define ERB_ID_DOPS            0x04
-#define ERB_ID_VELOCITY_NED    0x05
-#define ERB_ID_SPACE_INFO      0x06 // not used, reduces stack usage
-#define ERB_ID_RTK             0x07 // RTK, TODO
+#define ERB_SYNC_1                    0x45 // E
+#define ERB_SYNC_2                    0x52 // R
 
+#define ERB_ID_VERSION                0x01
+#define ERB_ID_GEODIC_POSITION        0x02
+#define ERB_ID_NAV_STATUS             0x03
+#define ERB_ID_DOPS                   0x04
+#define ERB_ID_VELOCITY_NED           0x05
+#define ERB_ID_SPACE_INFO             0x06 // not used, reduces stack usage
+#define ERB_ID_RTK                    0x07 // RTK, TODO
 
-#define EMLID_UNUSED(x) (void)x;
 
-#define GPS_PI 3.141592653589793238462643383280f
+#define EMLID_UNUSED(x)               (void)x;
 
-#define AUTO_DETECT_MAX_READ_SENTENCE 5  // if more detect succeed
+#define GPS_PI                        3.141592653589793238462643383280f
 
+#define AUTO_DETECT_MAX_READ_SENTENCE 5 // if more detect succeed
 
 GPSDriverEmlidReach::GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback_user,
-		sensor_gps_s *gps_position, satellite_info_s *satellite_info) :
-	GPSHelper(callback, callback_user),
-	_gps_position(gps_position), _satellite_info(satellite_info)
-{}
-
-
-int
-GPSDriverEmlidReach::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	// TODO RTK
-	if (config.output_mode != OutputMode::GPS) {
-		GPS_WARN("EMLIDREACH: Unsupported Output Mode %i", (int)config.output_mode);
-		return -1;
-	}
-
-	unsigned baud_allowed[] {57600, 115200, 230400};
-
-	for (unsigned i = 0; i < sizeof(baud_allowed) / sizeof(baud_allowed[0]); i++) {
-		if (baudrate > 0 && baudrate != baud_allowed[i]) {
-			continue;
-		}
-
-		_sentence_cnt = 0;
-		_erb_decode_state  = ERB_State::init;
-		_erb_payload_len = 0;
-		_erb_buff_cnt = 0;
-
-		if (GPSHelper::setBaudrate(baud_allowed[i]) != 0) {
-			continue;
-		}
-
-		if (! testConnection()) {
-			continue;
-		}
-
-		baudrate = baud_allowed[i];
-		return 0;
-	}
-
-	return -1;
+                                         sensor_gps_s *gps_position, satellite_info_s *satellite_info) :
+    GPSHelper(callback, callback_user),
+    _gps_position(gps_position), _satellite_info(satellite_info) {
 }
 
+int GPSDriverEmlidReach::configure(unsigned &baudrate, const GPSConfig &config) {
+    // TODO RTK
+    if (config.output_mode != OutputMode::GPS) {
+        GPS_WARN("EMLIDREACH: Unsupported Output Mode %i", (int)config.output_mode);
+        return -1;
+    }
 
-bool
-GPSDriverEmlidReach::testConnection()
-{
-	_testing_connection = true;
-	receive(500);
-	_testing_connection = false;
-	return _sentence_cnt >= AUTO_DETECT_MAX_READ_SENTENCE;
-}
-
-
-int
-GPSDriverEmlidReach::receive(unsigned timeout)
-{
-	uint8_t read_buff[GPS_READ_BUFFER_SIZE];
-	unsigned read_buff_len = 0;
-	uint8_t read_ind = 0;
-	int return_status = 0;
-
-	gps_abstime time_started = gps_absolute_time();
-
-	while (true) {
-		// read from serial, timeout may be truncated further in read()
-		read_buff_len = read(read_buff, sizeof(read_buff), timeout);
+    unsigned baud_allowed[]{57600, 115200, 230400};
 
-		if (read_buff_len > 0) {
-			read_ind = 0;
-		}
+    for (unsigned i = 0; i < sizeof(baud_allowed) / sizeof(baud_allowed[0]); i++) {
+        if (baudrate > 0 && baudrate != baud_allowed[i]) {
+            continue;
+        }
 
-		// process data in buffer return by read()
-		while (read_ind < read_buff_len) {
-			int ret = 0;
-			ret = erbParseChar(read_buff[read_ind++]);
+        _sentence_cnt     = 0;
+        _erb_decode_state = ERB_State::init;
+        _erb_payload_len  = 0;
+        _erb_buff_cnt     = 0;
 
-			if (ret > 0) {
-				_sentence_cnt ++;
+        if (GPSHelper::setBaudrate(baud_allowed[i]) != 0) {
+            continue;
+        }
 
-				// when testig connection, we care about syntax not semantic
-				if (! _testing_connection) {
-					return_status = handleErbSentence();
-				}
-			}
-		}
+        if (!testConnection()) {
+            continue;
+        }
 
-		if (return_status > 0) {
-			return return_status;
-		}
+        baudrate = baud_allowed[i];
+        return 0;
+    }
 
-		/* abort after timeout if no useful packets received */
-		if (time_started + timeout * 1000 < gps_absolute_time()) {
-			// device timed out
-			return -1;
-		}
-	}
+    return -1;
+}
 
-	return -1;
+bool GPSDriverEmlidReach::testConnection() {
+    _testing_connection = true;
+    receive(500);
+    _testing_connection = false;
+    return _sentence_cnt >= AUTO_DETECT_MAX_READ_SENTENCE;
 }
 
+int GPSDriverEmlidReach::receive(unsigned timeout) {
+    uint8_t  read_buff[GPS_READ_BUFFER_SIZE];
+    unsigned read_buff_len = 0;
+    uint8_t  read_ind      = 0;
+    int      return_status = 0;
+
+    gps_abstime time_started = gps_absolute_time();
+
+    while (true) {
+        // read from serial, timeout may be truncated further in read()
+        read_buff_len = read(read_buff, sizeof(read_buff), timeout);
+
+        if (read_buff_len > 0) {
+            read_ind = 0;
+        }
+
+        // process data in buffer return by read()
+        while (read_ind < read_buff_len) {
+            int ret = 0;
+            ret     = erbParseChar(read_buff[read_ind++]);
+
+            if (ret > 0) {
+                _sentence_cnt++;
+
+                // when testig connection, we care about syntax not semantic
+                if (!_testing_connection) {
+                    return_status = handleErbSentence();
+                }
+            }
+        }
+
+        if (return_status > 0) {
+            return return_status;
+        }
+
+        /* abort after timeout if no useful packets received */
+        if (time_started + timeout * 1000 < gps_absolute_time()) {
+            // device timed out
+            return -1;
+        }
+    }
+
+    return -1;
+}
 
 //// ERB
 
-int
-GPSDriverEmlidReach::erbParseChar(uint8_t b)
-{
-	int ret = 0;
-	uint8_t *buff_ptr = (uint8_t *)&_erb_buff;
+int GPSDriverEmlidReach::erbParseChar(uint8_t b) {
+    int      ret      = 0;
+    uint8_t *buff_ptr = (uint8_t *)&_erb_buff;
 
-	switch (_erb_decode_state) {
-	case ERB_State::init:
-		if (b == ERB_SYNC_1) {
-			_erb_buff_cnt = 0;
-			buff_ptr[_erb_buff_cnt ++] = b;
-			_erb_decode_state = ERB_State::got_sync_1;
-		}
+    switch (_erb_decode_state) {
+    case ERB_State::init:
+        if (b == ERB_SYNC_1) {
+            _erb_buff_cnt             = 0;
+            buff_ptr[_erb_buff_cnt++] = b;
+            _erb_decode_state         = ERB_State::got_sync_1;
+        }
 
-		break;
+        break;
 
-	case ERB_State::got_sync_1:
-		if (b == ERB_SYNC_2) {
-			buff_ptr[_erb_buff_cnt ++] = b;
-			_erb_decode_state = ERB_State::got_sync_2;
+    case ERB_State::got_sync_1:
+        if (b == ERB_SYNC_2) {
+            buff_ptr[_erb_buff_cnt++] = b;
+            _erb_decode_state         = ERB_State::got_sync_2;
 
-		} else {
-			_erb_decode_state = ERB_State::init;
-		}
+        } else {
+            _erb_decode_state = ERB_State::init;
+        }
 
-		break;
+        break;
 
-	case ERB_State::got_sync_2:
-		if (b >= ERB_ID_VERSION && b <= ERB_ID_RTK) {
-			buff_ptr[_erb_buff_cnt ++] = b;
-			_erb_decode_state = ERB_State::got_id;
+    case ERB_State::got_sync_2:
+        if (b >= ERB_ID_VERSION && b <= ERB_ID_RTK) {
+            buff_ptr[_erb_buff_cnt++] = b;
+            _erb_decode_state         = ERB_State::got_id;
 
-			if (b == ERB_ID_SPACE_INFO || b == ERB_ID_RTK) {
-				//ignore those
-				_erb_decode_state = ERB_State::init;
-			}
+            if (b == ERB_ID_SPACE_INFO || b == ERB_ID_RTK) {
+                //ignore those
+                _erb_decode_state = ERB_State::init;
+            }
 
-		} else {
-			_erb_decode_state = ERB_State::init;
-		}
+        } else {
+            _erb_decode_state = ERB_State::init;
+        }
 
-		break;
+        break;
 
-	case ERB_State::got_id:
-		buff_ptr[_erb_buff_cnt ++] = b;
-		_erb_decode_state = ERB_State::got_len_1;
-		_erb_payload_len = b;
-		break;
+    case ERB_State::got_id:
+        buff_ptr[_erb_buff_cnt++] = b;
+        _erb_decode_state         = ERB_State::got_len_1;
+        _erb_payload_len          = b;
+        break;
 
-	case ERB_State::got_len_1:
-		buff_ptr[_erb_buff_cnt ++] = b;
-		_erb_decode_state = ERB_State::got_len_2;
-		_erb_payload_len = (b << 8) | _erb_payload_len;
-		break;
+    case ERB_State::got_len_1:
+        buff_ptr[_erb_buff_cnt++] = b;
+        _erb_decode_state         = ERB_State::got_len_2;
+        _erb_payload_len          = (b << 8) | _erb_payload_len;
+        break;
 
-	case ERB_State::got_len_2:
-		if (_erb_buff_cnt > ERB_SENTENCE_MAX_LEN - sizeof(erb_checksum_t)) {
-			_erb_decode_state = ERB_State::init;
+    case ERB_State::got_len_2:
+        if (_erb_buff_cnt > ERB_SENTENCE_MAX_LEN - sizeof(erb_checksum_t)) {
+            _erb_decode_state = ERB_State::init;
 
-		} else {
-			buff_ptr[_erb_buff_cnt ++] = b;
-		}
+        } else {
+            buff_ptr[_erb_buff_cnt++] = b;
+        }
 
-		if (_erb_buff_cnt >= _erb_payload_len + static_cast<unsigned>(ERB_HEADER_LEN)) {
-			_erb_decode_state = ERB_State::got_payload;
-		}
+        if (_erb_buff_cnt >= _erb_payload_len + static_cast<unsigned>(ERB_HEADER_LEN)) {
+            _erb_decode_state = ERB_State::got_payload;
+        }
 
-		break;
+        break;
 
-	case ERB_State::got_payload:
-		_erb_checksum.ck_a = b;
-		_erb_decode_state = ERB_State::got_CK_A;
-		break;
+    case ERB_State::got_payload:
+        _erb_checksum.ck_a = b;
+        _erb_decode_state  = ERB_State::got_CK_A;
+        break;
 
-	case ERB_State::got_CK_A:
-		_erb_checksum.ck_b = b;
+    case ERB_State::got_CK_A:
+        _erb_checksum.ck_b = b;
 
-		uint8_t cka = 0, ckb = 0;
+        uint8_t cka = 0, ckb = 0;
 
-		for (unsigned i = 2; i < _erb_payload_len + static_cast<unsigned>(ERB_HEADER_LEN); i++) {
-			cka += buff_ptr[i];
-			ckb += cka;
-		}
+        for (unsigned i = 2; i < _erb_payload_len + static_cast<unsigned>(ERB_HEADER_LEN); i++) {
+            cka += buff_ptr[i];
+            ckb += cka;
+        }
 
-		if (cka == _erb_checksum.ck_a && ckb == _erb_checksum.ck_b) {
-			ret = 1;
+        if (cka == _erb_checksum.ck_a && ckb == _erb_checksum.ck_b) {
+            ret = 1;
 
-		} else {
-			ret = 0;
-		}
+        } else {
+            ret = 0;
+        }
 
-		_erb_decode_state = ERB_State::init;
-		break;
-	}
+        _erb_decode_state = ERB_State::init;
+        break;
+    }
 
-	return ret;
+    return ret;
 }
 
-int
-GPSDriverEmlidReach::handleErbSentence()
-{
-	int ret = 0;
-
-	if (_erb_buff.header.id == ERB_ID_VERSION) {
-		//GPS_INFO("EMLIDREACH: ERB version: %d.%d.%d", _buff[ERB_HEADER_LEN + 4], _buff[ERB_HEADER_LEN + 5], _buff[ERB_HEADER_LEN + 6]);
-
-	} else if (_erb_buff.header.id == ERB_ID_GEODIC_POSITION) {
-
-		_gps_position->timestamp = gps_absolute_time();
+int GPSDriverEmlidReach::handleErbSentence() {
+    int ret = 0;
 
-		_last_POS_timeGPS = _erb_buff.payload.geodic_position.timeGPS;
-		_gps_position->lon = round(_erb_buff.payload.geodic_position.longitude * 1e7);
-		_gps_position->lat = round(_erb_buff.payload.geodic_position.latitude * 1e7);
-		_gps_position->alt_ellipsoid = round(_erb_buff.payload.geodic_position.altElipsoid * 1e3);
-		_gps_position->alt = round(_erb_buff.payload.geodic_position.altMeanSeaLevel * 1e3);
+    if (_erb_buff.header.id == ERB_ID_VERSION) {
+        //GPS_INFO("EMLIDREACH: ERB version: %d.%d.%d", _buff[ERB_HEADER_LEN + 4], _buff[ERB_HEADER_LEN + 5], _buff[ERB_HEADER_LEN + 6]);
 
-		_rate_count_lat_lon++;
+    } else if (_erb_buff.header.id == ERB_ID_GEODIC_POSITION) {
+        _gps_position->timestamp = gps_absolute_time();
 
-		_gps_position->eph = static_cast<float>(_erb_buff.payload.geodic_position.accHorizontal) * 1e-3f;
-		_gps_position->epv = static_cast<float>(_erb_buff.payload.geodic_position.accVertical) * 1e-3f;
+        _last_POS_timeGPS            = _erb_buff.payload.geodic_position.timeGPS;
+        _gps_position->lon           = round(_erb_buff.payload.geodic_position.longitude * 1e7);
+        _gps_position->lat           = round(_erb_buff.payload.geodic_position.latitude * 1e7);
+        _gps_position->alt_ellipsoid = round(_erb_buff.payload.geodic_position.altElipsoid * 1e3);
+        _gps_position->alt           = round(_erb_buff.payload.geodic_position.altMeanSeaLevel * 1e3);
 
-		_gps_position->vel_ned_valid = false;
-		_gps_position->hdop = _hdop;
-		_gps_position->vdop = _vdop;
-		_gps_position->satellites_used = _satellites_used;
-		_gps_position->fix_type = _fix_type;
+        _rate_count_lat_lon++;
 
-		_POS_received = true;
+        _gps_position->eph = static_cast<float>(_erb_buff.payload.geodic_position.accHorizontal) * 1e-3f;
+        _gps_position->epv = static_cast<float>(_erb_buff.payload.geodic_position.accVertical) * 1e-3f;
 
-	} else if (_erb_buff.header.id == ERB_ID_NAV_STATUS) {
+        _gps_position->vel_ned_valid   = false;
+        _gps_position->hdop            = _hdop;
+        _gps_position->vdop            = _vdop;
+        _gps_position->satellites_used = _satellites_used;
+        _gps_position->fix_type        = _fix_type;
 
-		_fix_type = _erb_buff.payload.navigation_status.fixType;
-		_fix_status = _erb_buff.payload.navigation_status.fixStatus;
-		_satellites_used = _erb_buff.payload.navigation_status.numSatUsed;
+        _POS_received = true;
 
-		if (_fix_type == 0) {
-			// no Fix
-			_fix_type = 0;
+    } else if (_erb_buff.header.id == ERB_ID_NAV_STATUS) {
+        _fix_type        = _erb_buff.payload.navigation_status.fixType;
+        _fix_status      = _erb_buff.payload.navigation_status.fixStatus;
+        _satellites_used = _erb_buff.payload.navigation_status.numSatUsed;
 
-		} else if (_fix_type == 1) {
-			// Single
-			_fix_type = (_satellites_used > 4) ? 3 : 2;
+        if (_fix_type == 0) {
+            // no Fix
+            _fix_type = 0;
 
-		} else if (_fix_type == 2) {
-			// RTK Float
-			_fix_type = 5;
+        } else if (_fix_type == 1) {
+            // Single
+            _fix_type = (_satellites_used > 4) ? 3 : 2;
 
-		} else if (_fix_type == 3) {
-			// RTK Fix
-			_fix_type = 6;
+        } else if (_fix_type == 2) {
+            // RTK Float
+            _fix_type = 5;
 
-		} else {
-			_fix_type = 0;
-		}
+        } else if (_fix_type == 3) {
+            // RTK Fix
+            _fix_type = 6;
 
+        } else {
+            _fix_type = 0;
+        }
 
-	} else if (_erb_buff.header.id == ERB_ID_DOPS) {
 
-		_hdop = static_cast<float>(_erb_buff.payload.dop.dopHorizontal) / 100.0f;
-		_vdop = static_cast<float>(_erb_buff.payload.dop.dopVertical) / 100.0f;
+    } else if (_erb_buff.header.id == ERB_ID_DOPS) {
+        _hdop = static_cast<float>(_erb_buff.payload.dop.dopHorizontal) / 100.0f;
+        _vdop = static_cast<float>(_erb_buff.payload.dop.dopVertical) / 100.0f;
 
-	} else if (_erb_buff.header.id == ERB_ID_VELOCITY_NED) {
+    } else if (_erb_buff.header.id == ERB_ID_VELOCITY_NED) {
+        _last_VEL_timeGPS = _erb_buff.payload.ned_velocity.timeGPS;
 
-		_last_VEL_timeGPS = _erb_buff.payload.ned_velocity.timeGPS;
+        _gps_position->vel_n_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velN) / 100.0f;
+        _gps_position->vel_e_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velE) / 100.0f;
+        _gps_position->vel_d_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velD) / 100.0f;
 
-		_gps_position->vel_n_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velN) / 100.0f;
-		_gps_position->vel_e_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velE) / 100.0f;
-		_gps_position->vel_d_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.velD) / 100.0f;
+        _gps_position->vel_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.speed) / 100.0f;
+        _gps_position->cog_rad = (static_cast<float>(_erb_buff.payload.ned_velocity.heading) / 1e5f) * GPS_PI / 180.0f;
 
-		_gps_position->vel_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.speed) / 100.0f;
-		_gps_position->cog_rad = (static_cast<float>(_erb_buff.payload.ned_velocity.heading) / 1e5f) * GPS_PI / 180.0f;
+        _gps_position->s_variance_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.speedAccuracy) / 100.0f;
 
-		_gps_position->s_variance_m_s = static_cast<float>(_erb_buff.payload.ned_velocity.speedAccuracy) / 100.0f;
+        _gps_position->vel_ned_valid = true;
+        _rate_count_vel++;
 
-		_gps_position->vel_ned_valid = true;
-		_rate_count_vel++;
+        _VEL_received = true;
 
-		_VEL_received = true;
+    } else if (_erb_buff.header.id == ERB_ID_SPACE_INFO) {
+    } else if (_erb_buff.header.id == ERB_ID_RTK) {
+    } else {
+        //GPS_WARN("EMLIDREACH: ERB ID not known: %d", _erb_buff.header.id);
+    }
 
-	} else if (_erb_buff.header.id == ERB_ID_SPACE_INFO) {
+    // Emlid doc: "If position and velocity are valid, it takes value 0x01, else it takes 0x00"
+    if (_fix_status == 1
+        && _POS_received && _VEL_received
+        && _last_POS_timeGPS == _last_VEL_timeGPS) {
+        ret           = 1;
+        _POS_received = false;
+        _VEL_received = false;
+    }
 
-	} else if (_erb_buff.header.id == ERB_ID_RTK) {
-
-	} else {
-		//GPS_WARN("EMLIDREACH: ERB ID not known: %d", _erb_buff.header.id);
-	}
-
-	// Emlid doc: "If position and velocity are valid, it takes value 0x01, else it takes 0x00"
-	if (_fix_status == 1
-	    && _POS_received && _VEL_received
-	    && _last_POS_timeGPS == _last_VEL_timeGPS) {
-		ret = 1;
-		_POS_received = false;
-		_VEL_received = false;
-	}
-
-	return ret;
+    return ret;
 }
-
-
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.h b/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.h
index 170be950eb..1af0f42f81 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/emlid_reach.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file emlid_reach.h
@@ -46,172 +23,166 @@
 //   https://docs.emlid.com/reachm-plus/
 //   https://files.emlid.com/ERB.pdf
 
-#define ERB_HEADER_LEN           5
+#define ERB_HEADER_LEN 5
 // Not using ERB_ID_SPACE_INFO, so use a smaller buff length
-#define ERB_SENTENCE_MAX_LEN     (sizeof(erb_message_t))
+#define ERB_SENTENCE_MAX_LEN (sizeof(erb_message_t))
 
-#define MAX_CONST(a, b) ((a>b) ? a : b)
+#define MAX_CONST(a, b)      ((a > b) ? a : b)
 
 // Emlid ERB message definition
 #pragma pack(push, 1)
 
 typedef struct {
-	uint8_t		sync1;
-	uint8_t		sync2;
-	uint8_t		id;
-	uint16_t	length;
+    uint8_t  sync1;
+    uint8_t  sync2;
+    uint8_t  id;
+    uint16_t length;
 } erb_header_t;
 
 typedef struct {
-	uint8_t			ck_a;
-	uint8_t			ck_b;
+    uint8_t ck_a;
+    uint8_t ck_b;
 } erb_checksum_t;
 
 typedef struct {
-	uint32_t		timeGPS;
-	uint8_t			verH;
-	uint8_t			verM;
-	uint8_t			verL;
-	erb_checksum_t	checksum;
+    uint32_t       timeGPS;
+    uint8_t        verH;
+    uint8_t        verM;
+    uint8_t        verL;
+    erb_checksum_t checksum;
 } erb_version_t;
 
 typedef struct {
-	uint32_t		timeGPS;
-	double			longitude;
-	double			latitude;
-	double			altElipsoid;
-	double			altMeanSeaLevel;
-	uint32_t		accHorizontal;
-	uint32_t		accVertical;
-	erb_checksum_t	checksum;
+    uint32_t       timeGPS;
+    double         longitude;
+    double         latitude;
+    double         altElipsoid;
+    double         altMeanSeaLevel;
+    uint32_t       accHorizontal;
+    uint32_t       accVertical;
+    erb_checksum_t checksum;
 } erb_geodic_position_t;
 
 typedef struct {
-	uint32_t		timeGPS;
-	uint16_t		weekGPS;
-	uint8_t			fixType;
-	uint8_t			fixStatus;
-	uint8_t			numSatUsed;
-	erb_checksum_t	checksum;
+    uint32_t       timeGPS;
+    uint16_t       weekGPS;
+    uint8_t        fixType;
+    uint8_t        fixStatus;
+    uint8_t        numSatUsed;
+    erb_checksum_t checksum;
 } erb_navigation_status_t;
 
 typedef struct {
-	uint32_t		timeGPS;
-	uint16_t		dopGeometric;
-	uint16_t		dopPosition;
-	uint16_t		dopVertical;
-	uint16_t		dopHorizontal;
-	erb_checksum_t	checksum;
+    uint32_t       timeGPS;
+    uint16_t       dopGeometric;
+    uint16_t       dopPosition;
+    uint16_t       dopVertical;
+    uint16_t       dopHorizontal;
+    erb_checksum_t checksum;
 } erb_dop_t;
 
 typedef struct {
-	uint32_t		timeGPS;
-	int32_t			velN;
-	int32_t			velE;
-	int32_t			velD;
-	uint32_t		speed;
-	int32_t			heading;
-	uint32_t		speedAccuracy;
-	erb_checksum_t	checksum;
+    uint32_t       timeGPS;
+    int32_t        velN;
+    int32_t        velE;
+    int32_t        velD;
+    uint32_t       speed;
+    int32_t        heading;
+    uint32_t       speedAccuracy;
+    erb_checksum_t checksum;
 } erb_ned_velocity_t;
 
 typedef union {
-	erb_version_t			version;
-	erb_geodic_position_t	geodic_position;
-	erb_navigation_status_t	navigation_status;
-	erb_dop_t				dop;
-	erb_ned_velocity_t		ned_velocity;
+    erb_version_t           version;
+    erb_geodic_position_t   geodic_position;
+    erb_navigation_status_t navigation_status;
+    erb_dop_t               dop;
+    erb_ned_velocity_t      ned_velocity;
 } erb_payload_t;
 
 typedef struct {
-	erb_header_t	header;
-	erb_payload_t	payload;
+    erb_header_t  header;
+    erb_payload_t payload;
 } erb_message_t;
 
 #pragma pack(pop)
 
-
 /**
  * Driver class for Emlid Reach
  * Populates caller provided sensor_gps_s
  * Some ERB messages are cached and correlated by timestamp before publishing it
  */
-class GPSDriverEmlidReach : public GPSHelper
-{
+class GPSDriverEmlidReach : public GPSHelper {
 public:
-	GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback_user,
-			    sensor_gps_s *gps_position,
-			    satellite_info_s *satellite_info
-			   );
+    GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback_user,
+                        sensor_gps_s     *gps_position,
+                        satellite_info_s *satellite_info);
 
-	virtual ~GPSDriverEmlidReach() = default;
+    virtual ~GPSDriverEmlidReach() = default;
 
-	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int receive(unsigned timeout) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
-
-	enum class ERB_State {
-		init = 0,
-		got_sync_1,  // E
-		got_sync_2,  // R
-		got_id,
-		got_len_1,
-		got_len_2,
-		got_payload,
-		got_CK_A
-	};
+    enum class ERB_State {
+        init = 0,
+        got_sync_1, // E
+        got_sync_2, // R
+        got_id,
+        got_len_1,
+        got_len_2,
+        got_payload,
+        got_CK_A
+    };
 
 
-	/** NMEA parser state machine */
-	ERB_State _erb_decode_state;
+    /** NMEA parser state machine */
+    ERB_State _erb_decode_state;
 
-	/** Buffer used by parser to build ERB sentences */
-	erb_message_t _erb_buff{};
-	uint16_t _erb_buff_cnt{};
+    /** Buffer used by parser to build ERB sentences */
+    erb_message_t _erb_buff{};
+    uint16_t      _erb_buff_cnt{};
 
-	/** Buffer used by parser to build ERB checksum */
-	erb_checksum_t _erb_checksum{};
-	uint8_t _erb_checksum_cnt{};
+    /** Buffer used by parser to build ERB checksum */
+    erb_checksum_t _erb_checksum{};
+    uint8_t        _erb_checksum_cnt{};
 
-	/** Pointer provided by caller, ie gps.cpp */
-	sensor_gps_s *_gps_position {nullptr};
-	/** Pointer provided by caller, gps.cpp */
-	satellite_info_s *_satellite_info {nullptr};
+    /** Pointer provided by caller, ie gps.cpp */
+    sensor_gps_s *_gps_position{nullptr};
+    /** Pointer provided by caller, gps.cpp */
+    satellite_info_s *_satellite_info{nullptr};
 
-	bool _testing_connection{false};
-	/** counts decoded sentence when testing connection */
-	unsigned _sentence_cnt{0};
+    bool _testing_connection{false};
+    /** counts decoded sentence when testing connection */
+    unsigned _sentence_cnt{0};
 
-	uint16_t _erb_payload_len{0};
+    uint16_t _erb_payload_len{0};
 
-	uint32_t _last_POS_timeGPS{0};
-	uint32_t _last_VEL_timeGPS{0};
-	bool _POS_received{false};
-	bool _VEL_received{false};
+    uint32_t _last_POS_timeGPS{0};
+    uint32_t _last_VEL_timeGPS{0};
+    bool     _POS_received{false};
+    bool     _VEL_received{false};
 
 
-	///// ERB messages caches /////
-	uint8_t _fix_type{0};
-	uint8_t _fix_status{0};
-	uint8_t _satellites_used{0};
-	float _hdop{0};
-	float _vdop{0};
+    ///// ERB messages caches /////
+    uint8_t _fix_type{0};
+    uint8_t _fix_status{0};
+    uint8_t _satellites_used{0};
+    float   _hdop{0};
+    float   _vdop{0};
 
 
-	/** Feed ERB parser with received bytes from serial
+    /** Feed ERB parser with received bytes from serial
 	 * @return len of decoded message, 0 if not completed, -1 if error
 	 */
-	int erbParseChar(uint8_t b);
+    int erbParseChar(uint8_t b);
 
-	/** ERB sentence into sensor_gps_s or satellite_info_s, to be used by GPSHelper
+    /** ERB sentence into sensor_gps_s or satellite_info_s, to be used by GPSHelper
 	 *  @return 1 if gps_position updated, 2 for satellite_info_s (can be bit OR), 0 for nothing
 	 */
-	int handleErbSentence();
+    int handleErbSentence();
 
-	void computeNedVelocity();
-
-	bool testConnection();
+    void computeNedVelocity();
 
+    bool testConnection();
 };
-
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.cpp
index 6d8ab89f04..56d0b9d824 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <stdlib.h>
 #include <stdio.h>
@@ -44,697 +21,710 @@
 
 
 /* ms, timeout for waiting for a response*/
-#define FEMTO_RESPONSE_TIMEOUT		200
+#define FEMTO_RESPONSE_TIMEOUT 200
 
-#define FEMTO_MSG_MAX_LENGTH		256
+#define FEMTO_MSG_MAX_LENGTH   256
 /* Femtomes ID for UAV output message */
-#define FEMTO_MSG_ID_UAVGPS 		8001
-#define FEMTO_MSG_ID_RTCM3          784
-#define FEMTO_MSG_ID_GPGGA          218
-#define FEMTO_MSG_ID_UAVSTATUS		8017
+#define FEMTO_MSG_ID_UAVGPS    8001
+#define FEMTO_MSG_ID_RTCM3     784
+#define FEMTO_MSG_ID_GPGGA     218
+#define FEMTO_MSG_ID_UAVSTATUS 8017
 
 /* Femto uavgps message frame premble 3 bytes*/
-#define FEMTO_PREAMBLE1			0xaa
-#define FEMTO_PREAMBLE2			0x44
-#define FEMTO_PREAMBLE3			0x12
+#define FEMTO_PREAMBLE1 0xaa
+#define FEMTO_PREAMBLE2 0x44
+#define FEMTO_PREAMBLE3 0x12
 
-#define MIN(X,Y)	((X) < (Y) ? (X) : (Y))
+#define MIN(X, Y)       ((X) < (Y) ? (X) : (Y))
 #define FEMTO_UNUSED(x) (void)x;
 
 #if defined _FMTOMES_DEBUG
-#define FEMTO_INFO(...)			{GPS_INFO(__VA_ARGS__);}
-#define FEMTO_DEBUG(...)		{GPS_WARN(__VA_ARGS__);}
-#define FEMTO_ERR(...)			{GPS_ERR(__VA_ARGS__);}
+#   define FEMTO_INFO(...) \
+       { GPS_INFO(__VA_ARGS__); }
+#   define FEMTO_DEBUG(...) \
+       { GPS_WARN(__VA_ARGS__); }
+#   define FEMTO_ERR(...) \
+       { GPS_ERR(__VA_ARGS__); }
 #else
-#define FEMTO_INFO(...)			{(void)0;}
-#define FEMTO_DEBUG(...)		{(void)0;}
-#define FEMTO_ERR(...)			{GPS_WARN(__VA_ARGS__);}
+#   define FEMTO_INFO(...) \
+       { (void)0; }
+#   define FEMTO_DEBUG(...) \
+       { (void)0; }
+#   define FEMTO_ERR(...) \
+       { GPS_WARN(__VA_ARGS__); }
 #endif
 
 
 GPSDriverFemto::GPSDriverFemto(GPSCallbackPtr callback, void *callback_user,
-			       struct sensor_gps_s *gps_position,
-			       satellite_info_s *satellite_info,
-			       float heading_offset) :
-	GPSBaseStationSupport(callback, callback_user),
-	_gps_position(gps_position),
-	_satellite_info(satellite_info),
-	_heading_offset(heading_offset)
-{
-	decodeInit();
+                               struct sensor_gps_s *gps_position,
+                               satellite_info_s    *satellite_info,
+                               float                heading_offset) :
+    GPSBaseStationSupport(callback, callback_user),
+    _gps_position(gps_position),
+    _satellite_info(satellite_info),
+    _heading_offset(heading_offset) {
+    decodeInit();
 }
 
-GPSDriverFemto::~GPSDriverFemto()
-{
-	delete _rtcm_parsing;
+GPSDriverFemto::~GPSDriverFemto() {
+    delete _rtcm_parsing;
 }
 
-int GPSDriverFemto::handleMessage(int len)
-{
-	int ret = 0;
-	uint16_t messageid = _femto_msg.header.femto_header.messageid;
-
-	if (messageid == FEMTO_MSG_ID_UAVGPS) { /**< uavgpsB*/
-		memcpy(&_femto_uav_gps, _femto_msg.data, sizeof(femto_uav_gps_t));
-
-		_gps_position->time_utc_usec = _femto_uav_gps.time_utc_usec;
-		_gps_position->lat = _femto_uav_gps.lat;
-		_gps_position->lon = _femto_uav_gps.lon;
-		_gps_position->alt = _femto_uav_gps.alt;
-		_gps_position->alt_ellipsoid = _femto_uav_gps.alt_ellipsoid;
-		_gps_position->s_variance_m_s = _femto_uav_gps.s_variance_m_s;
-		_gps_position->c_variance_rad = _femto_uav_gps.c_variance_rad;
-		_gps_position->eph = _femto_uav_gps.eph;
-		_gps_position->epv = _femto_uav_gps.epv;
-		_gps_position->hdop = _femto_uav_gps.hdop;
-		_gps_position->vdop = _femto_uav_gps.vdop;
-		_gps_position->noise_per_ms = _femto_uav_gps.noise_per_ms;
-		_gps_position->jamming_indicator = _femto_uav_gps.jamming_indicator;
-		_gps_position->vel_m_s = _femto_uav_gps.vel_m_s;
-		_gps_position->vel_n_m_s = _femto_uav_gps.vel_n_m_s;
-		_gps_position->vel_e_m_s = _femto_uav_gps.vel_e_m_s;
-		_gps_position->vel_d_m_s = _femto_uav_gps.vel_d_m_s;
-		_gps_position->cog_rad = _femto_uav_gps.cog_rad;
-		_gps_position->timestamp_time_relative = _femto_uav_gps.timestamp_time_relative;
-		_gps_position->fix_type = _femto_uav_gps.fix_type;
-		_gps_position->vel_ned_valid = _femto_uav_gps.vel_ned_valid;
-		_gps_position->satellites_used = _femto_uav_gps.satellites_used;
-
-		if (_femto_uav_gps.heading_type == 6) {
-			float heading = _femto_uav_gps.heading;
-			heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
-			heading -= _heading_offset; // range: [-pi, 3pi]
-
-			if (heading > M_PI_F) {
-				heading -= 2.f * M_PI_F; // final range is [-pi, pi]
-			}
-
-			_gps_position->heading = heading;
-
-		} else {
-			_gps_position->heading = NAN;
-		}
-
-		_gps_position->timestamp = gps_absolute_time();
-
-		ret = 1;
-
-	} else if (_satellite_info && messageid == FEMTO_MSG_ID_UAVSTATUS) {	/**< set satellite info */
-		const femto_uav_status_t *uav_status = (const femto_uav_status_t *)_femto_msg.data;
-
-		_satellite_info->count = MIN(uav_status->sat_number, satellite_info_s::SAT_INFO_MAX_SATELLITES);
-
-		for (int8_t i = 0; i < _satellite_info->count; i++) {
-			_satellite_info->svid[i] = uav_status->sat_status[i].svid;
-			_satellite_info->used[i] = 1;
-			_satellite_info->elevation[i] = uav_status->sat_status[i].ele;
-			_satellite_info->azimuth[i] = uav_status->sat_status[i].azi;
-			_satellite_info->snr[i] = uav_status->sat_status[i].cn0;
-			_satellite_info->prn[i] = uav_status->sat_status[i].system_id;
-		}
-
-		ret = 2;
-
-	} else if (OutputMode::RTCM == _output_mode
-		   && messageid == FEMTO_MSG_ID_GPGGA
-		   && (memcmp(_femto_msg.data + 3, "GGA,", 3) == 0)) { /**< GPGGA only used in base station, for survey-in */
-		int uiCalcComma = 0;
-
-		for (int i = 0 ; i < len; i++) {
-			if (_femto_msg.data[i] == ',') { uiCalcComma++; }
-		}
-
-		if (uiCalcComma == 14) {
-			char *bufptr = (char *)(_femto_msg.data + 6);
-			char *endp = nullptr;
-			double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
-			int num_of_sv = 0, fix_quality = 0;
-			double hdop = 99.9;
-			char ns = '?', ew = '?';
-
-			if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-			if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-			if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
-
-			if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
-
-			if (ns == 'S') {
-				lat = -lat;
-			}
-
-			if (ew == 'W') {
-				lon = -lon;
-			}
-
-			FEMTO_UNUSED(ashtech_time)
-			FEMTO_UNUSED(hdop)
-
-			if (!_correction_output_activated && 7 == fix_quality) {
-				_survey_in_start = 0;	/**< finished survey-in */
-
-				lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
-				lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
-				alt = alt * 1000;
-
-				sendSurveyInStatusUpdate(false, true, lat, lon, (float)alt);
-				activateRTCMOutput();
-			}
-
-			if (_satellite_info) {
-				_satellite_info->count = (uint8_t)num_of_sv;    /**< base station satellite count */
-			}
-
-			ret = 2;
-		}
-	}
-
-	// handle survey-in status update
-	if (_survey_in_start != 0) {
-		const gps_abstime now = gps_absolute_time();
-		uint32_t survey_in_duration = (now - _survey_in_start) / 1000000;
-
-		if (survey_in_duration != _base_settings.settings.survey_in.min_dur) {
-			_base_settings.settings.survey_in.min_dur = survey_in_duration;
-			sendSurveyInStatusUpdate(true, false);
-		}
-	}
-
-	return ret;
+int GPSDriverFemto::handleMessage(int len) {
+    int      ret       = 0;
+    uint16_t messageid = _femto_msg.header.femto_header.messageid;
+
+    if (messageid == FEMTO_MSG_ID_UAVGPS) { /**< uavgpsB*/
+        memcpy(&_femto_uav_gps, _femto_msg.data, sizeof(femto_uav_gps_t));
+
+        _gps_position->time_utc_usec           = _femto_uav_gps.time_utc_usec;
+        _gps_position->lat                     = _femto_uav_gps.lat;
+        _gps_position->lon                     = _femto_uav_gps.lon;
+        _gps_position->alt                     = _femto_uav_gps.alt;
+        _gps_position->alt_ellipsoid           = _femto_uav_gps.alt_ellipsoid;
+        _gps_position->s_variance_m_s          = _femto_uav_gps.s_variance_m_s;
+        _gps_position->c_variance_rad          = _femto_uav_gps.c_variance_rad;
+        _gps_position->eph                     = _femto_uav_gps.eph;
+        _gps_position->epv                     = _femto_uav_gps.epv;
+        _gps_position->hdop                    = _femto_uav_gps.hdop;
+        _gps_position->vdop                    = _femto_uav_gps.vdop;
+        _gps_position->noise_per_ms            = _femto_uav_gps.noise_per_ms;
+        _gps_position->jamming_indicator       = _femto_uav_gps.jamming_indicator;
+        _gps_position->vel_m_s                 = _femto_uav_gps.vel_m_s;
+        _gps_position->vel_n_m_s               = _femto_uav_gps.vel_n_m_s;
+        _gps_position->vel_e_m_s               = _femto_uav_gps.vel_e_m_s;
+        _gps_position->vel_d_m_s               = _femto_uav_gps.vel_d_m_s;
+        _gps_position->cog_rad                 = _femto_uav_gps.cog_rad;
+        _gps_position->timestamp_time_relative = _femto_uav_gps.timestamp_time_relative;
+        _gps_position->fix_type                = _femto_uav_gps.fix_type;
+        _gps_position->vel_ned_valid           = _femto_uav_gps.vel_ned_valid;
+        _gps_position->satellites_used         = _femto_uav_gps.satellites_used;
+
+        if (_femto_uav_gps.heading_type == 6) {
+            float heading  = _femto_uav_gps.heading;
+            heading       *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
+            heading       -= _heading_offset; // range: [-pi, 3pi]
+
+            if (heading > M_PI_F) {
+                heading -= 2.f * M_PI_F; // final range is [-pi, pi]
+            }
+
+            _gps_position->heading = heading;
+
+        } else {
+            _gps_position->heading = NAN;
+        }
+
+        _gps_position->timestamp = gps_absolute_time();
+
+        ret = 1;
+
+    } else if (_satellite_info && messageid == FEMTO_MSG_ID_UAVSTATUS) { /**< set satellite info */
+        const femto_uav_status_t *uav_status = (const femto_uav_status_t *)_femto_msg.data;
+
+        _satellite_info->count = MIN(uav_status->sat_number, satellite_info_s::SAT_INFO_MAX_SATELLITES);
+
+        for (int8_t i = 0; i < _satellite_info->count; i++) {
+            _satellite_info->svid[i]      = uav_status->sat_status[i].svid;
+            _satellite_info->used[i]      = 1;
+            _satellite_info->elevation[i] = uav_status->sat_status[i].ele;
+            _satellite_info->azimuth[i]   = uav_status->sat_status[i].azi;
+            _satellite_info->snr[i]       = uav_status->sat_status[i].cn0;
+            _satellite_info->prn[i]       = uav_status->sat_status[i].system_id;
+        }
+
+        ret = 2;
+
+    } else if (OutputMode::RTCM == _output_mode
+               && messageid == FEMTO_MSG_ID_GPGGA
+               && (memcmp(_femto_msg.data + 3, "GGA,", 3) == 0)) { /**< GPGGA only used in base station, for survey-in */
+        int uiCalcComma = 0;
+
+        for (int i = 0; i < len; i++) {
+            if (_femto_msg.data[i] == ',') {
+                uiCalcComma++;
+            }
+        }
+
+        if (uiCalcComma == 14) {
+            char  *bufptr       = (char *)(_femto_msg.data + 6);
+            char  *endp         = nullptr;
+            double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+            int    num_of_sv = 0, fix_quality = 0;
+            double hdop = 99.9;
+            char   ns = '?', ew = '?';
+
+            if (bufptr && *(++bufptr) != ',') {
+                ashtech_time = strtod(bufptr, &endp);
+                bufptr       = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                lat    = strtod(bufptr, &endp);
+                bufptr = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                ns = *(bufptr++);
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                lon    = strtod(bufptr, &endp);
+                bufptr = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                ew = *(bufptr++);
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                fix_quality = strtol(bufptr, &endp, 10);
+                bufptr      = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                num_of_sv = strtol(bufptr, &endp, 10);
+                bufptr    = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                hdop   = strtod(bufptr, &endp);
+                bufptr = endp;
+            }
+
+            if (bufptr && *(++bufptr) != ',') {
+                alt    = strtod(bufptr, &endp);
+                bufptr = endp;
+            }
+
+            if (ns == 'S') {
+                lat = -lat;
+            }
+
+            if (ew == 'W') {
+                lon = -lon;
+            }
+
+            FEMTO_UNUSED(ashtech_time)
+            FEMTO_UNUSED(hdop)
+
+            if (!_correction_output_activated && 7 == fix_quality) {
+                _survey_in_start = 0; /**< finished survey-in */
+
+                lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
+                lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
+                alt = alt * 1000;
+
+                sendSurveyInStatusUpdate(false, true, lat, lon, (float)alt);
+                activateRTCMOutput();
+            }
+
+            if (_satellite_info) {
+                _satellite_info->count = (uint8_t)num_of_sv; /**< base station satellite count */
+            }
+
+            ret = 2;
+        }
+    }
+
+    // handle survey-in status update
+    if (_survey_in_start != 0) {
+        const gps_abstime now                = gps_absolute_time();
+        uint32_t          survey_in_duration = (now - _survey_in_start) / 1000000;
+
+        if (survey_in_duration != _base_settings.settings.survey_in.min_dur) {
+            _base_settings.settings.survey_in.min_dur = survey_in_duration;
+            sendSurveyInStatusUpdate(true, false);
+        }
+    }
+
+    return ret;
 }
 
-void GPSDriverFemto::receiveWait(unsigned timeout_min)
-{
-	gps_abstime time_started = gps_absolute_time();
-
-	while (gps_absolute_time() < time_started + timeout_min * 1000) {
-		receive(timeout_min);
-	}
+void GPSDriverFemto::receiveWait(unsigned timeout_min) {
+    gps_abstime time_started = gps_absolute_time();
 
+    while (gps_absolute_time() < time_started + timeout_min * 1000) {
+        receive(timeout_min);
+    }
 }
 
-int GPSDriverFemto::receive(unsigned timeout)
-{
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
+int GPSDriverFemto::receive(unsigned timeout) {
+    uint8_t buf[GPS_READ_BUFFER_SIZE];
 
-	/* timeout additional to poll */
-	uint64_t time_started = gps_absolute_time();
-	int j = 0;
-	int bytes_count = 0;
+    /* timeout additional to poll */
+    uint64_t time_started = gps_absolute_time();
+    int      j            = 0;
+    int      bytes_count  = 0;
 
-	while (true) {
+    while (true) {
+        /* pass received bytes to the packet decoder */
+        while (j < bytes_count) {
+            int l = 0;
 
-		/* pass received bytes to the packet decoder */
-		while (j < bytes_count) {
-			int l = 0;
-
-			if ((l = parseChar(buf[j])) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
+            if ((l = parseChar(buf[j])) > 0) {
+                /* return to configure during configuration or to the gps driver during normal work
 				 * if a packet has arrived */
-				int ret = handleMessage(l);
+                int ret = handleMessage(l);
 
-				if (ret > 0) {
-					_decode_state = FemtoDecodeState::pream_ble1;
-					return ret;
-				}
-			}
+                if (ret > 0) {
+                    _decode_state = FemtoDecodeState::pream_ble1;
+                    return ret;
+                }
+            }
 
-			j++;
-		}
+            j++;
+        }
 
-		/* everything is read */
-		j = bytes_count = 0;
+        /* everything is read */
+        j = bytes_count = 0;
 
-		/* then poll or read for new data */
-		int ret = read(buf, sizeof(buf), timeout * 2);
+        /* then poll or read for new data */
+        int ret = read(buf, sizeof(buf), timeout * 2);
 
-		if (ret < 0) {
-			/* something went wrong when polling */
-			return -1;
+        if (ret < 0) {
+            /* something went wrong when polling */
+            return -1;
 
-		} else if (ret == 0) {
-			/* Timeout while polling or just nothing read if reading, let's
+        } else if (ret == 0) {
+            /* Timeout while polling or just nothing read if reading, let's
 			 * stay here, and use timeout below. */
 
-		} else {
-			/* if we have new data from GPS, go handle it */
-			bytes_count = ret;
-		}
-
-		/* in case we get crap from GPS or time out */
-		if (time_started + timeout * 1000 * 4 < gps_absolute_time()) {
-			FEMTO_DEBUG("Femto: timeout\n");
-			return -1;
-		}
-	}
+        } else {
+            /* if we have new data from GPS, go handle it */
+            bytes_count = ret;
+        }
+
+        /* in case we get crap from GPS or time out */
+        if (time_started + timeout * 1000 * 4 < gps_absolute_time()) {
+            FEMTO_DEBUG("Femto: timeout\n");
+            return -1;
+        }
+    }
 }
 
-#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
-
-int GPSDriverFemto::parseChar(uint8_t temp)
-{
-	int iRet = 0;
-
-	if (_output_mode == OutputMode::GPS) {
-
-		switch (_decode_state) {
-		case FemtoDecodeState::pream_ble1:
-			if (temp == FEMTO_PREAMBLE1) {
-				_decode_state = FemtoDecodeState::pream_ble2;
-				_femto_msg.read = 0;
-			}
-
-			break;
-
-		case FemtoDecodeState::pream_ble2:
-			if (temp == FEMTO_PREAMBLE2) {
-				_decode_state = FemtoDecodeState::pream_ble3;
-
-			} else {
-				_decode_state = FemtoDecodeState::pream_ble1;
-			}
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A' - 0xA)))
 
-			break;
+int GPSDriverFemto::parseChar(uint8_t temp) {
+    int iRet = 0;
 
-		case FemtoDecodeState::pream_ble3:
-			if (temp == FEMTO_PREAMBLE3) {
-				_decode_state = FemtoDecodeState::head_length;
+    if (_output_mode == OutputMode::GPS) {
+        switch (_decode_state) {
+        case FemtoDecodeState::pream_ble1:
+            if (temp == FEMTO_PREAMBLE1) {
+                _decode_state   = FemtoDecodeState::pream_ble2;
+                _femto_msg.read = 0;
+            }
 
-			} else {
-				_decode_state = FemtoDecodeState::pream_ble1;
-			}
+            break;
 
-			break;
+        case FemtoDecodeState::pream_ble2:
+            if (temp == FEMTO_PREAMBLE2) {
+                _decode_state = FemtoDecodeState::pream_ble3;
 
-		case FemtoDecodeState::head_length:
-			_femto_msg.header.data[0] = FEMTO_PREAMBLE1;
-			_femto_msg.header.data[1] = FEMTO_PREAMBLE2;
-			_femto_msg.header.data[2] = FEMTO_PREAMBLE3;
-			_femto_msg.header.data[3] = temp;
-			_femto_msg.header.femto_header.headerlength = temp;
-			_decode_state = FemtoDecodeState::head_data;
-			_femto_msg.read = 4;
-			break;
+            } else {
+                _decode_state = FemtoDecodeState::pream_ble1;
+            }
+
+            break;
 
-		case FemtoDecodeState::head_data:
-			if (_femto_msg.read >= sizeof(_femto_msg.header.data)) {
-				_decode_state = FemtoDecodeState::pream_ble1;
-				break;
-			}
+        case FemtoDecodeState::pream_ble3:
+            if (temp == FEMTO_PREAMBLE3) {
+                _decode_state = FemtoDecodeState::head_length;
 
-			_femto_msg.header.data[_femto_msg.read] = temp;
-			_femto_msg.read++;
+            } else {
+                _decode_state = FemtoDecodeState::pream_ble1;
+            }
 
-			if (_femto_msg.read >= _femto_msg.header.femto_header.headerlength) {
-				_decode_state = FemtoDecodeState::data;
-			}
+            break;
 
-			break;
+        case FemtoDecodeState::head_length:
+            _femto_msg.header.data[0]                   = FEMTO_PREAMBLE1;
+            _femto_msg.header.data[1]                   = FEMTO_PREAMBLE2;
+            _femto_msg.header.data[2]                   = FEMTO_PREAMBLE3;
+            _femto_msg.header.data[3]                   = temp;
+            _femto_msg.header.femto_header.headerlength = temp;
+            _decode_state                               = FemtoDecodeState::head_data;
+            _femto_msg.read                             = 4;
+            break;
 
-		case FemtoDecodeState::data:
-			if (_femto_msg.read >= FEMTO_MSG_MAX_LENGTH) {
-				_decode_state = FemtoDecodeState::pream_ble1;
-				break;
-			}
+        case FemtoDecodeState::head_data:
+            if (_femto_msg.read >= sizeof(_femto_msg.header.data)) {
+                _decode_state = FemtoDecodeState::pream_ble1;
+                break;
+            }
 
-			_femto_msg.data[_femto_msg.read - _femto_msg.header.femto_header.headerlength] = temp;
-			_femto_msg.read++;
-
-			if (_femto_msg.read >= (_femto_msg.header.femto_header.messagelength + _femto_msg.header.femto_header.headerlength)) {
-				_decode_state = FemtoDecodeState::crc1;
-			}
-
-			break;
-
-		case FemtoDecodeState::crc1:
-			_femto_msg.crc = (uint32_t)(temp << 0);
-			_decode_state = FemtoDecodeState::crc2;
-			break;
-
-		case FemtoDecodeState::crc2:
-			_femto_msg.crc += (uint32_t)(temp << 8);
-			_decode_state = FemtoDecodeState::crc3;
-			break;
-
-		case FemtoDecodeState::crc3:
-			_femto_msg.crc += (uint32_t)(temp << 16);
-			_decode_state = FemtoDecodeState::crc4;
-			break;
-
-		case FemtoDecodeState::crc4: {
-				_femto_msg.crc += (uint32_t)(temp << 24);
-				_decode_state = FemtoDecodeState::pream_ble1;
+            _femto_msg.header.data[_femto_msg.read] = temp;
+            _femto_msg.read++;
 
-				uint32_t crc = calculateCRC32((uint32_t)_femto_msg.header.femto_header.headerlength,
-							      (uint8_t *)&_femto_msg.header.data, (uint32_t)0);
-				crc = calculateCRC32((uint32_t)_femto_msg.header.femto_header.messagelength, (uint8_t *)&_femto_msg.data[0], crc);
+            if (_femto_msg.read >= _femto_msg.header.femto_header.headerlength) {
+                _decode_state = FemtoDecodeState::data;
+            }
 
-				if (_femto_msg.crc == crc) {
-					iRet = _femto_msg.read;
-
-				} else {
-					FEMTO_DEBUG("Femto: data packet is bad");
-				}
-			}
-			break;
-
-		default:
-			break;
-		}
-
-	} else {	/**< RTCM mode */
+            break;
+
+        case FemtoDecodeState::data:
+            if (_femto_msg.read >= FEMTO_MSG_MAX_LENGTH) {
+                _decode_state = FemtoDecodeState::pream_ble1;
+                break;
+            }
 
-		switch (_decode_state) {
-		case FemtoDecodeState::pream_ble1:
-			if (temp == '$') {
-				_decode_state = FemtoDecodeState::pream_nmea_got_sync1;
-				_femto_msg.read = 0;
-				_femto_msg.data[_femto_msg.read++] = temp;
-
-			} else if (temp == RTCM3_PREAMBLE && _rtcm_parsing) {
-				_decode_state = FemtoDecodeState::decode_rtcm3;
-				_rtcm_parsing->addByte(temp);
-			}
-
-			break;
-
-		case FemtoDecodeState::pream_nmea_got_sync1:
-			if (temp == '$') {
-				_decode_state = FemtoDecodeState::pream_nmea_got_sync1;
-				_femto_msg.read = 0;
-
-			} else if (temp == '*') {
-				_decode_state = FemtoDecodeState::pream_nmea_got_asteriks;
-
-			} else if (temp == RTCM3_PREAMBLE && _rtcm_parsing) {
-				_decode_state = FemtoDecodeState::decode_rtcm3;
-				_rtcm_parsing->addByte(temp);
-			}
-
-			if (_femto_msg.read >= (sizeof(_femto_msg.data) - 5)) {
-				FEMTO_DEBUG("buffer overflow")
-				_decode_state = FemtoDecodeState::pream_ble1;
-				_femto_msg.read = 0;
+            _femto_msg.data[_femto_msg.read - _femto_msg.header.femto_header.headerlength] = temp;
+            _femto_msg.read++;
+
+            if (_femto_msg.read >= (_femto_msg.header.femto_header.messagelength + _femto_msg.header.femto_header.headerlength)) {
+                _decode_state = FemtoDecodeState::crc1;
+            }
+
+            break;
+
+        case FemtoDecodeState::crc1:
+            _femto_msg.crc = (uint32_t)(temp << 0);
+            _decode_state  = FemtoDecodeState::crc2;
+            break;
+
+        case FemtoDecodeState::crc2:
+            _femto_msg.crc += (uint32_t)(temp << 8);
+            _decode_state   = FemtoDecodeState::crc3;
+            break;
+
+        case FemtoDecodeState::crc3:
+            _femto_msg.crc += (uint32_t)(temp << 16);
+            _decode_state   = FemtoDecodeState::crc4;
+            break;
 
-			} else {
-				_femto_msg.data[_femto_msg.read++] = temp;
-			}
+        case FemtoDecodeState::crc4: {
+            _femto_msg.crc += (uint32_t)(temp << 24);
+            _decode_state   = FemtoDecodeState::pream_ble1;
+
+            uint32_t crc = calculateCRC32((uint32_t)_femto_msg.header.femto_header.headerlength,
+                                          (uint8_t *)&_femto_msg.header.data, (uint32_t)0);
+            crc          = calculateCRC32((uint32_t)_femto_msg.header.femto_header.messagelength, (uint8_t *)&_femto_msg.data[0], crc);
+
+            if (_femto_msg.crc == crc) {
+                iRet = _femto_msg.read;
+
+            } else {
+                FEMTO_DEBUG("Femto: data packet is bad");
+            }
+        } break;
+
+        default:
+            break;
+        }
+
+    } else { /**< RTCM mode */
+
+        switch (_decode_state) {
+        case FemtoDecodeState::pream_ble1:
+            if (temp == '$') {
+                _decode_state                      = FemtoDecodeState::pream_nmea_got_sync1;
+                _femto_msg.read                    = 0;
+                _femto_msg.data[_femto_msg.read++] = temp;
+
+            } else if (temp == RTCM3_PREAMBLE && _rtcm_parsing) {
+                _decode_state = FemtoDecodeState::decode_rtcm3;
+                _rtcm_parsing->addByte(temp);
+            }
+
+            break;
+
+        case FemtoDecodeState::pream_nmea_got_sync1:
+            if (temp == '$') {
+                _decode_state   = FemtoDecodeState::pream_nmea_got_sync1;
+                _femto_msg.read = 0;
+
+            } else if (temp == '*') {
+                _decode_state = FemtoDecodeState::pream_nmea_got_asteriks;
+
+            } else if (temp == RTCM3_PREAMBLE && _rtcm_parsing) {
+                _decode_state = FemtoDecodeState::decode_rtcm3;
+                _rtcm_parsing->addByte(temp);
+            }
+
+            if (_femto_msg.read >= (sizeof(_femto_msg.data) - 5)) {
+                FEMTO_DEBUG("buffer overflow")
+                _decode_state   = FemtoDecodeState::pream_ble1;
+                _femto_msg.read = 0;
+
+            } else {
+                _femto_msg.data[_femto_msg.read++] = temp;
+            }
+
+            break;
 
-			break;
+        case FemtoDecodeState::pream_nmea_got_asteriks:
+            _femto_msg.data[_femto_msg.read++] = temp;
+            _decode_state                      = FemtoDecodeState::pream_nmea_got_first_cs_byte;
+            break;
+
+        case FemtoDecodeState::pream_nmea_got_first_cs_byte: {
+            _femto_msg.data[_femto_msg.read++] = temp;
+            uint8_t  checksum                  = 0;
+            uint8_t *buffer                    = _femto_msg.data + 1;
+            uint8_t *bufend                    = _femto_msg.data + _femto_msg.read - 3;
+
+            for (; buffer < bufend; buffer++) {
+                checksum ^= *buffer;
+            }
 
-		case FemtoDecodeState::pream_nmea_got_asteriks:
-			_femto_msg.data[_femto_msg.read++] = temp;
-			_decode_state = FemtoDecodeState::pream_nmea_got_first_cs_byte;
-			break;
+            if ((HEXDIGIT_CHAR(checksum >> 4) == *(_femto_msg.data + _femto_msg.read - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_femto_msg.data + _femto_msg.read - 1))) {
+                iRet                                     = _femto_msg.read;
+                _femto_msg.header.femto_header.messageid = FEMTO_MSG_ID_GPGGA;
+                FEMTO_DEBUG("Femto: got NMEA message with length %i", _femto_msg.read)
+            }
+
+            decodeInit();
+            break;
+        }
+
+        case FemtoDecodeState::decode_rtcm3:
+            if (_rtcm_parsing->addByte(temp)) {
+                FEMTO_DEBUG("Femto: got RTCM message with length %i", (int)_rtcm_parsing->messageLength())
+                gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
+                decodeInit();
+            }
+
+            break;
+
+        default:
+            break;
+        }
+    }
 
-		case FemtoDecodeState::pream_nmea_got_first_cs_byte: {
-				_femto_msg.data[_femto_msg.read++] = temp;
-				uint8_t checksum = 0;
-				uint8_t *buffer = _femto_msg.data + 1;
-				uint8_t *bufend = _femto_msg.data + _femto_msg.read - 3;
-
-				for (; buffer < bufend; buffer++) {
-					checksum ^= *buffer;
-				}
 
-				if ((HEXDIGIT_CHAR(checksum >> 4) == *(_femto_msg.data + _femto_msg.read - 2)) &&
-				    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_femto_msg.data + _femto_msg.read - 1))) {
-					iRet = _femto_msg.read;
-					_femto_msg.header.femto_header.messageid = FEMTO_MSG_ID_GPGGA;
-					FEMTO_DEBUG("Femto: got NMEA message with length %i", _femto_msg.read)
-				}
-
-				decodeInit();
-				break;
-			}
-
-		case FemtoDecodeState::decode_rtcm3:
-			if (_rtcm_parsing->addByte(temp)) {
-				FEMTO_DEBUG("Femto: got RTCM message with length %i", (int)_rtcm_parsing->messageLength())
-				gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
-				decodeInit();
-			}
-
-			break;
-
-		default:
-			break;
-		}
+    return iRet;
+}
 
-	}
+void GPSDriverFemto::decodeInit() {
+    _decode_state = FemtoDecodeState::pream_ble1;
 
+    /** init or reset rtcm parsing */
+    if (_output_mode == OutputMode::RTCM) {
+        if (!_rtcm_parsing) {
+            _rtcm_parsing = new RTCMParsing();
+        }
 
-	return iRet;
+        if (_rtcm_parsing) {
+            _rtcm_parsing->reset();
+        }
+    }
 }
 
-void GPSDriverFemto::decodeInit()
-{
-	_decode_state = FemtoDecodeState::pream_ble1;
+int GPSDriverFemto::writeAckedCommandFemto(const char *command, const char *reply, const unsigned int timeout) {
+    /**< write command*/
+    write(command, strlen(command));
+    /**< wait for reply*/
+    uint8_t     buf[GPS_READ_BUFFER_SIZE];
+    gps_abstime time_started = gps_absolute_time();
 
-	/** init or reset rtcm parsing */
-	if (_output_mode == OutputMode::RTCM) {
-		if (!_rtcm_parsing) {
-			_rtcm_parsing = new RTCMParsing();
-		}
+    while (time_started + timeout * 2000 > gps_absolute_time()) {
+        int ret              = read(buf, sizeof(buf), timeout);
+        buf[sizeof(buf) - 1] = 0;
 
-		if (_rtcm_parsing) {
-			_rtcm_parsing->reset();
-		}
-	}
-}
+        if (ret > 0 && strstr((char *)buf, reply) != nullptr) {
+            FEMTO_DEBUG("Femto: command reply success: %s", command);
+            return 0;
+        }
+    }
 
-int GPSDriverFemto::writeAckedCommandFemto(const char *command, const char *reply, const unsigned int timeout)
-{
-	/**< write command*/
-	write(command, strlen(command));
-	/**< wait for reply*/
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
-	gps_abstime time_started = gps_absolute_time();
-
-	while (time_started + timeout * 2000 > gps_absolute_time()) {
-		int ret = read(buf, sizeof(buf), timeout);
-		buf[sizeof(buf) - 1] = 0;
-
-		if (ret > 0 && strstr((char *)buf, reply) != nullptr) {
-			FEMTO_DEBUG("Femto: command reply success: %s", command);
-			return 0;
-		}
-	}
-
-	return -1;
+    return -1;
 }
 
-int GPSDriverFemto::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	FEMTO_DEBUG("Femto: configure gps driver")
+int GPSDriverFemto::configure(unsigned &baudrate, const GPSConfig &config) {
+    FEMTO_DEBUG("Femto: configure gps driver")
 
-	if (config.output_mode != OutputMode::GPS && config.output_mode != OutputMode::RTCM) {
-		FEMTO_DEBUG("Femto: Unsupported Output Mode %i", (int)config.output_mode);
-		return -1;
-	}
+    if (config.output_mode != OutputMode::GPS && config.output_mode != OutputMode::RTCM) {
+        FEMTO_DEBUG("Femto: Unsupported Output Mode %i", (int)config.output_mode);
+        return -1;
+    }
 
-	_output_mode = config.output_mode;
-	_configure_done = false;
-	_correction_output_activated = false;
-	/** Try different baudrates (115200 is the default for Femtomes) and request the baudrate that we want.	 */
-	const unsigned baudrates_to_try[] = {115200};
-	bool success = false;
+    _output_mode                 = config.output_mode;
+    _configure_done              = false;
+    _correction_output_activated = false;
+    /** Try different baudrates (115200 is the default for Femtomes) and request the baudrate that we want.	 */
+    const unsigned baudrates_to_try[] = {115200};
+    bool           success            = false;
 
-	unsigned test_baudrate = 0;
+    unsigned test_baudrate = 0;
 
-	for (unsigned int baud_i = 0; !success && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
-		test_baudrate = baudrates_to_try[baud_i];
+    for (unsigned int baud_i = 0; !success && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
+        test_baudrate = baudrates_to_try[baud_i];
 
-		if (baudrate > 0 && baudrate != test_baudrate) {
-			continue; /**< skip to next baudrate*/
-		}
+        if (baudrate > 0 && baudrate != test_baudrate) {
+            continue; /**< skip to next baudrate*/
+        }
 
-		setBaudrate(test_baudrate);
+        setBaudrate(test_baudrate);
 
-		FEMTO_DEBUG("Femto: baudrate set to %i", test_baudrate);
+        FEMTO_DEBUG("Femto: baudrate set to %i", test_baudrate);
 
-		for (int run = 0; run < 2; ++run) { /** try several times*/
-			if (writeAckedCommandFemto("UNLOGALL THISPORT\r\n", "<UNLOGALL OK", FEMTO_RESPONSE_TIMEOUT) == 0 &&
-			    writeAckedCommandFemto("VERSION\r\n", "<VERSION OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-				FEMTO_DEBUG("Femto: got port for baudrate %i", test_baudrate);
-				success = true;
-				break;
-			}
-		}
-	}
+        for (int run = 0; run < 2; ++run) { /** try several times*/
+            if (writeAckedCommandFemto("UNLOGALL THISPORT\r\n", "<UNLOGALL OK", FEMTO_RESPONSE_TIMEOUT) == 0 && writeAckedCommandFemto("VERSION\r\n", "<VERSION OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+                FEMTO_DEBUG("Femto: got port for baudrate %i", test_baudrate);
+                success = true;
+                break;
+            }
+        }
+    }
 
-	if (!success) {
-		FEMTO_DEBUG("Femto: gps driver configure failed,no port for baudrate %i", test_baudrate);
-		return -1;
-	}
+    if (!success) {
+        FEMTO_DEBUG("Femto: gps driver configure failed,no port for baudrate %i", test_baudrate);
+        return -1;
+    }
 
-	/**
+    /**
 	* We successfully got a response and know to which port we are connected. Now set the desired baudrate
 	* if it's different from the current one.
 	*/
-	const unsigned desired_baudrate = 115200; /**< changing this requires also changing the SPD command*/
+    const unsigned desired_baudrate = 115200; /**< changing this requires also changing the SPD command*/
 
-	baudrate = test_baudrate;
+    baudrate = test_baudrate;
 
-	if (baudrate != desired_baudrate) {
-		baudrate = desired_baudrate;
-		const char baud_config[] = "com 115200\r\n"; // configure baudrate to 115200
-		write(baud_config, sizeof(baud_config));
-		decodeInit();
-		receiveWait(200);
-		decodeInit();
-		setBaudrate(baudrate);
+    if (baudrate != desired_baudrate) {
+        baudrate                 = desired_baudrate;
+        const char baud_config[] = "com 115200\r\n"; // configure baudrate to 115200
+        write(baud_config, sizeof(baud_config));
+        decodeInit();
+        receiveWait(200);
+        decodeInit();
+        setBaudrate(baudrate);
 
-		success = false;
+        success = false;
 
-		for (int run = 0; run < 10; ++run) {
-			/** We ask for the port config again. If we get a reply, we know that the changed settings work.*/
-			if (writeAckedCommandFemto("UNLOGALL THISPORT\r\n", "<UNLOGALL OK", FEMTO_RESPONSE_TIMEOUT) == 0 &&
-			    writeAckedCommandFemto("VERSION\r\n", "<VERSION OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-				success = true;
-				break;
-			}
-		}
+        for (int run = 0; run < 10; ++run) {
+            /** We ask for the port config again. If we get a reply, we know that the changed settings work.*/
+            if (writeAckedCommandFemto("UNLOGALL THISPORT\r\n", "<UNLOGALL OK", FEMTO_RESPONSE_TIMEOUT) == 0 && writeAckedCommandFemto("VERSION\r\n", "<VERSION OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+                success = true;
+                break;
+            }
+        }
 
-		if (!success) {
-			return -1;
-		}
+        if (!success) {
+            return -1;
+        }
 
-	} else {
-		decodeInit();
-	}
+    } else {
+        decodeInit();
+    }
 
-	if (_output_mode == OutputMode::GPS) {
-		if (writeAckedCommandFemto("LOG UAVGPSB 0.1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-			FEMTO_DEBUG("Femto: command LOG UAVGPSB 0.1 success");
+    if (_output_mode == OutputMode::GPS) {
+        if (writeAckedCommandFemto("LOG UAVGPSB 0.1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+            FEMTO_DEBUG("Femto: command LOG UAVGPSB 0.1 success");
 
-			/** 20Hz need authorization in femtomes device */
-			if (writeAckedCommandFemto("LOG UAVGPSB 0.05\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-				FEMTO_DEBUG("Femto: command LOG UAVGPSB 0.05 success");
+            /** 20Hz need authorization in femtomes device */
+            if (writeAckedCommandFemto("LOG UAVGPSB 0.05\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+                FEMTO_DEBUG("Femto: command LOG UAVGPSB 0.05 success");
 
-			} else {
-				FEMTO_ERR("Femto: command LOG UAVGPSB 0.05 failed,maybe no authorization");
-			}
+            } else {
+                FEMTO_ERR("Femto: command LOG UAVGPSB 0.05 failed,maybe no authorization");
+            }
 
-		} else {
-			FEMTO_ERR("Femto: command LOG UAVGPSB 0.1 failed");
-		}
+        } else {
+            FEMTO_ERR("Femto: command LOG UAVGPSB 0.1 failed");
+        }
 
-		if (_satellite_info) {
-			if (writeAckedCommandFemto("LOG UAVSTATUSB 1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-				FEMTO_DEBUG("Femto: command LOG UAVSTATUSB 1 success");
+        if (_satellite_info) {
+            if (writeAckedCommandFemto("LOG UAVSTATUSB 1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+                FEMTO_DEBUG("Femto: command LOG UAVSTATUSB 1 success");
 
-			} else {
-				FEMTO_ERR("Femto: command LOG UAVSTATUSB 1 failed");
-			}
-		}
+            } else {
+                FEMTO_ERR("Femto: command LOG UAVSTATUSB 1 failed");
+            }
+        }
 
 
-	} else {	/**< RTCM mode for base station */
-		activateCorrectionOutput();
-	}
+    } else { /**< RTCM mode for base station */
+        activateCorrectionOutput();
+    }
 
-	_configure_done = true;
-	FEMTO_DEBUG("Femto: gps driver configure done")
+    _configure_done = true;
+    FEMTO_DEBUG("Femto: gps driver configure done")
 
-	return 0;
+    return 0;
 }
 
-void GPSDriverFemto::activateCorrectionOutput()
-{
-	if (_output_mode != OutputMode::RTCM) {
-		return;	/**< only for base station */
-	}
-
-	char buffer[100];
-
-	if (_base_settings.type == BaseSettingsType::survey_in) {
-		FEMTO_DEBUG("Femto: enabling survey-in")
+void GPSDriverFemto::activateCorrectionOutput() {
+    if (_output_mode != OutputMode::RTCM) {
+        return; /**< only for base station */
+    }
 
-		if (writeAckedCommandFemto("POSAVE ON \r\n", "<POSAVE OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-			FEMTO_DEBUG("Femto: command POSAVE ON success")
+    char buffer[100];
 
-			if (writeAckedCommandFemto("LOG GPGGA 1 \r\n", "<LOG OK",
-						   FEMTO_RESPONSE_TIMEOUT) == 0) { /**< for updating GPS satellite count of RTK */
-				FEMTO_DEBUG("Femto: command LOG GPGGA 1 success")
+    if (_base_settings.type == BaseSettingsType::survey_in) {
+        FEMTO_DEBUG("Femto: enabling survey-in")
 
-			} else {
-				FEMTO_ERR("Femto: LOG GPGGA command failed")
-			}
+        if (writeAckedCommandFemto("POSAVE ON \r\n", "<POSAVE OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+            FEMTO_DEBUG("Femto: command POSAVE ON success")
 
-		} else {
-			FEMTO_ERR("Femto: command POSAVE ON failed")
-		}
+            if (writeAckedCommandFemto("LOG GPGGA 1 \r\n", "<LOG OK",
+                                       FEMTO_RESPONSE_TIMEOUT)
+                == 0) { /**< for updating GPS satellite count of RTK */
+                FEMTO_DEBUG("Femto: command LOG GPGGA 1 success")
 
-		_base_settings.settings.survey_in.min_dur = 0; // use it as counter how long survey-in has been active
-		_survey_in_start = gps_absolute_time();
-		sendSurveyInStatusUpdate(true, false);
+            } else {
+                FEMTO_ERR("Femto: LOG GPGGA command failed")
+            }
 
-	} else {
-		FEMTO_DEBUG("Femto: setting base station position")
+        } else {
+            FEMTO_ERR("Femto: command POSAVE ON failed")
+        }
 
-		const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
-		int len = snprintf(buffer, sizeof(buffer), "FIX POSITION %.8lf %.8lf %.5f\r\n",
-				   settings.latitude, settings.longitude, (double)settings.altitude);
+        _base_settings.settings.survey_in.min_dur = 0; // use it as counter how long survey-in has been active
+        _survey_in_start                          = gps_absolute_time();
+        sendSurveyInStatusUpdate(true, false);
 
-		if (len >= 0 && len < (int)(sizeof(buffer))) {
-			if (writeAckedCommandFemto(buffer, "FIX OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
-				FEMTO_DEBUG("Femto: command %s success", buffer)
-				activateRTCMOutput();
-				sendSurveyInStatusUpdate(false, true, settings.latitude, settings.longitude, settings.altitude);
+    } else {
+        FEMTO_DEBUG("Femto: setting base station position")
 
-				if (writeAckedCommandFemto("LOG GPGGA 1 \r\n", "<LOG OK",
-							   FEMTO_RESPONSE_TIMEOUT) == 0) { /**< for updating GPS satellite count of RTK */
-					FEMTO_DEBUG("Femto: command LOG GPGGA 1 success")
+        const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
+        int                          len      = snprintf(buffer, sizeof(buffer), "FIX POSITION %.8lf %.8lf %.5f\r\n",
+                                                         settings.latitude, settings.longitude, (double)settings.altitude);
 
-				} else {
-					FEMTO_ERR("Femto: LOG GPGGA command failed")
-				}
+        if (len >= 0 && len < (int)(sizeof(buffer))) {
+            if (writeAckedCommandFemto(buffer, "FIX OK", FEMTO_RESPONSE_TIMEOUT) == 0) {
+                FEMTO_DEBUG("Femto: command %s success", buffer)
+                activateRTCMOutput();
+                sendSurveyInStatusUpdate(false, true, settings.latitude, settings.longitude, settings.altitude);
 
-			} else {
-				FEMTO_ERR("Femto: fix base station position failed.")
-			}
+                if (writeAckedCommandFemto("LOG GPGGA 1 \r\n", "<LOG OK",
+                                           FEMTO_RESPONSE_TIMEOUT)
+                    == 0) { /**< for updating GPS satellite count of RTK */
+                    FEMTO_DEBUG("Femto: command LOG GPGGA 1 success")
 
-		} else {
-			FEMTO_DEBUG("Femto: snprintf failed (buffer too short)")
-		}
+                } else {
+                    FEMTO_ERR("Femto: LOG GPGGA command failed")
+                }
 
+            } else {
+                FEMTO_ERR("Femto: fix base station position failed.")
+            }
 
-	}
+        } else {
+            FEMTO_DEBUG("Femto: snprintf failed (buffer too short)")
+        }
+    }
 }
 
-void GPSDriverFemto::activateRTCMOutput()
-{
-	if (writeAckedCommandFemto("LOG RTCM 1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) != 0) {
-		FEMTO_ERR("Femto: command LOG RTCM failed")
+void GPSDriverFemto::activateRTCMOutput() {
+    if (writeAckedCommandFemto("LOG RTCM 1\r\n", "<LOG OK", FEMTO_RESPONSE_TIMEOUT) != 0) {
+        FEMTO_ERR("Femto: command LOG RTCM failed")
 
-	} else {
-		FEMTO_DEBUG("Femto: command LOG RTCM 1 success")
-	}
+    } else {
+        FEMTO_DEBUG("Femto: command LOG RTCM 1 success")
+    }
 
-	_correction_output_activated = true;
+    _correction_output_activated = true;
 }
 
 void GPSDriverFemto::sendSurveyInStatusUpdate(bool active, bool valid, double latitude, double longitude,
-		float altitude)
-{
-	SurveyInStatus status;
-	status.latitude = latitude;
-	status.longitude = longitude;
-	status.altitude = altitude;
-	status.duration = _base_settings.settings.survey_in.min_dur;
-	status.mean_accuracy = 0; // unknown
-	status.flags = (int)valid | ((int)active << 1);
-	surveyInStatus(status);
+                                              float altitude) {
+    SurveyInStatus status;
+    status.latitude      = latitude;
+    status.longitude     = longitude;
+    status.altitude      = altitude;
+    status.duration      = _base_settings.settings.survey_in.min_dur;
+    status.mean_accuracy = 0; // unknown
+    status.flags         = (int)valid | ((int)active << 1);
+    surveyInStatus(status);
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.h b/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.h
index bcdb3410f8..b6d268de4b 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/femtomes.h
@@ -1,36 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2019. All rights reserved.
- *   Author: Rui Zheng <ruizheng@femtomes.com>
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /** @file Femtomes protocol definitions */
 #pragma once
@@ -48,188 +24,187 @@ class RTCMParsing;
 * femto_uav_gps_t struct need to be packed
 */
 typedef struct {
-	uint64_t 	time_utc_usec;		/** Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0*/
-	int32_t 	lat;			/** Latitude in 1E-7 degrees*/
-	int32_t 	lon;			/** Longitude in 1E-7 degrees*/
-	int32_t 	alt;			/** Altitude in 1E-3 meters above MSL, (millimetres)*/
-	int32_t 	alt_ellipsoid;		/** Altitude in 1E-3 meters bove Ellipsoid, (millimetres)*/
-	float 		s_variance_m_s;		/** GPS speed accuracy estimate, (metres/sec)*/
-	float 		c_variance_rad;		/** GPS course accuracy estimate, (radians)*/
-	float 		eph;			/** GPS horizontal position accuracy (metres)*/
-	float 		epv;			/** GPS vertical position accuracy (metres)*/
-	float 		hdop;			/** Horizontal dilution of precision*/
-	float 		vdop;			/** Vertical dilution of precision*/
-	int32_t 	noise_per_ms;		/** GPS noise per millisecond*/
-	int32_t 	jamming_indicator;	/** indicates jamming is occurring*/
-	float 		vel_m_s;		/** GPS ground speed, (metres/sec)*/
-	float 		vel_n_m_s;		/** GPS North velocity, (metres/sec)*/
-	float 		vel_e_m_s;		/** GPS East velocity, (metres/sec)*/
-	float 		vel_d_m_s;		/** GPS Down velocity, (metres/sec)*/
-	float 		cog_rad;		/** Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)*/
-	int32_t 	timestamp_time_relative;/** timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)*/
-	float 		heading;		/** heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])*/
-	uint8_t 	fix_type;		/** 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/
-	bool 		vel_ned_valid;		/** True if NED velocity is valid*/
-	uint8_t 	satellites_used;	/** Number of satellites used*/
-	uint8_t		heading_type;		/**< 0 invalid,5 for float,6 for fix*/
+    uint64_t time_utc_usec;           /** Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0*/
+    int32_t  lat;                     /** Latitude in 1E-7 degrees*/
+    int32_t  lon;                     /** Longitude in 1E-7 degrees*/
+    int32_t  alt;                     /** Altitude in 1E-3 meters above MSL, (millimetres)*/
+    int32_t  alt_ellipsoid;           /** Altitude in 1E-3 meters bove Ellipsoid, (millimetres)*/
+    float    s_variance_m_s;          /** GPS speed accuracy estimate, (metres/sec)*/
+    float    c_variance_rad;          /** GPS course accuracy estimate, (radians)*/
+    float    eph;                     /** GPS horizontal position accuracy (metres)*/
+    float    epv;                     /** GPS vertical position accuracy (metres)*/
+    float    hdop;                    /** Horizontal dilution of precision*/
+    float    vdop;                    /** Vertical dilution of precision*/
+    int32_t  noise_per_ms;            /** GPS noise per millisecond*/
+    int32_t  jamming_indicator;       /** indicates jamming is occurring*/
+    float    vel_m_s;                 /** GPS ground speed, (metres/sec)*/
+    float    vel_n_m_s;               /** GPS North velocity, (metres/sec)*/
+    float    vel_e_m_s;               /** GPS East velocity, (metres/sec)*/
+    float    vel_d_m_s;               /** GPS Down velocity, (metres/sec)*/
+    float    cog_rad;                 /** Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)*/
+    int32_t  timestamp_time_relative; /** timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)*/
+    float    heading;                 /** heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])*/
+    uint8_t  fix_type;                /** 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/
+    bool     vel_ned_valid;           /** True if NED velocity is valid*/
+    uint8_t  satellites_used;         /** Number of satellites used*/
+    uint8_t  heading_type;            /**< 0 invalid,5 for float,6 for fix*/
 } femto_uav_gps_t;
 
 /**
 * femto_msg_header_t is femto data header
 */
 typedef struct {
-	uint8_t 	preamble[3];	/**< Frame header preamble 0xaa 0x44 0x12 */
-	uint8_t 	headerlength;	/**< Frame header length ,from the beginning 0xaa */
-	uint16_t 	messageid;     /**< Frame message id ,example the FEMTO_MSG_ID_UAVGPS 8001*/
-	uint8_t 	messagetype;	/**< Frame message id type */
-	uint8_t 	portaddr;	/**< Frame message port address */
-	uint16_t 	messagelength; /**< Frame message data length,from the beginning headerlength+1,end headerlength + messagelength*/
-	uint16_t 	sequence;
-	uint8_t 	idletime;	/**< Frame message idle module time */
-	uint8_t 	timestatus;
-	uint16_t 	week;
-	uint32_t 	tow;
-	uint32_t 	recvstatus;
-	uint16_t 	resv;
-	uint16_t 	recvswver;
+    uint8_t  preamble[3];   /**< Frame header preamble 0xaa 0x44 0x12 */
+    uint8_t  headerlength;  /**< Frame header length ,from the beginning 0xaa */
+    uint16_t messageid;     /**< Frame message id ,example the FEMTO_MSG_ID_UAVGPS 8001*/
+    uint8_t  messagetype;   /**< Frame message id type */
+    uint8_t  portaddr;      /**< Frame message port address */
+    uint16_t messagelength; /**< Frame message data length,from the beginning headerlength+1,end headerlength + messagelength*/
+    uint16_t sequence;
+    uint8_t  idletime;      /**< Frame message idle module time */
+    uint8_t  timestatus;
+    uint16_t week;
+    uint32_t tow;
+    uint32_t recvstatus;
+    uint16_t resv;
+    uint16_t recvswver;
 } femto_msg_header_t;
 
 /**
 *  uav status data
 */
 typedef struct {
-	int32_t     master_ant_status;
-	int32_t     slave_ant_status;
-	uint16_t    master_ant_power;
-	uint16_t    slave_ant_power;
-	uint32_t    jam_status;
-	uint32_t    spoofing_status;
-	uint16_t    reserved16_1;
-	uint16_t    diff_age;		    /**< in unit of second*/
-	uint32_t    base_id;
-	uint32_t    reserved32_1;
-	uint32_t    reserved32_2;
-	uint32_t    sat_number;
-	struct femto_uav_sat_status_data_t {
-		uint8_t   svid;
-		uint8_t   system_id;
-		uint8_t   cn0;		        /**< in unit of dB-Hz*/
-		uint8_t   ele;		        /**< in unit of degree*/
-		uint16_t  azi;		        /**< in unit of degree*/
-		uint16_t  status;
-	} sat_status[64];               /**< uav status of all satellites */
+    int32_t  master_ant_status;
+    int32_t  slave_ant_status;
+    uint16_t master_ant_power;
+    uint16_t slave_ant_power;
+    uint32_t jam_status;
+    uint32_t spoofing_status;
+    uint16_t reserved16_1;
+    uint16_t diff_age; /**< in unit of second*/
+    uint32_t base_id;
+    uint32_t reserved32_1;
+    uint32_t reserved32_2;
+    uint32_t sat_number;
+
+    struct femto_uav_sat_status_data_t {
+        uint8_t  svid;
+        uint8_t  system_id;
+        uint8_t  cn0; /**< in unit of dB-Hz*/
+        uint8_t  ele; /**< in unit of degree*/
+        uint16_t azi; /**< in unit of degree*/
+        uint16_t status;
+    } sat_status[64]; /**< uav status of all satellites */
 } femto_uav_status_t;
 
 /**
 * Analysis Femto uavgps frame header
 */
 typedef union {
-	femto_msg_header_t 	femto_header;
-	uint8_t 	 	data[28];
+    femto_msg_header_t femto_header;
+    uint8_t            data[28];
 } msg_header_t;
 
 /**
 * receive Femto complete uavgps frame
 */
 typedef struct {
-	uint8_t 		data[600];		/**< receive Frame message content */
-	uint32_t 		crc;			/**< receive Frame message crc 4 bytes */
-	msg_header_t 		header;			/**< receive Frame message header */
-	uint16_t 		read;			/**< receive Frame message read bytes count */
+    uint8_t      data[600]; /**< receive Frame message content */
+    uint32_t     crc;       /**< receive Frame message crc 4 bytes */
+    msg_header_t header;    /**< receive Frame message header */
+    uint16_t     read;      /**< receive Frame message read bytes count */
 } femto_msg_t;
 
 #pragma pack(pop)
 /*** END OF femtomes protocol binary message and payload definitions ***/
 
 enum class FemtoDecodeState {
-	pream_ble1,			/**< Frame header preamble first byte 0xaa */
-	pream_ble2,			/**< Frame header preamble second byte 0x44 */
-	pream_ble3,			/**< Frame header preamble third byte 0x12 */
-	head_length,			/**< Frame header length */
-	head_data,			/**< Frame header data */
-	data,				/**< Frame data */
-	crc1,				/**< Frame crc1 */
-	crc2,				/**< Frame crc2 */
-	crc3,				/**< Frame crc3 */
-	crc4,				/**< Frame crc4 */
-
-	pream_nmea_got_sync1,           /**< NMEA Frame '$' */
-	pream_nmea_got_asteriks,        /**< NMEA Frame '*' */
-	pream_nmea_got_first_cs_byte,   /**< NMEA Frame cs first byte */
-
-	decode_rtcm3                    /**< Frame rtcm3 */
+    pream_ble1,                   /**< Frame header preamble first byte 0xaa */
+    pream_ble2,                   /**< Frame header preamble second byte 0x44 */
+    pream_ble3,                   /**< Frame header preamble third byte 0x12 */
+    head_length,                  /**< Frame header length */
+    head_data,                    /**< Frame header data */
+    data,                         /**< Frame data */
+    crc1,                         /**< Frame crc1 */
+    crc2,                         /**< Frame crc2 */
+    crc3,                         /**< Frame crc3 */
+    crc4,                         /**< Frame crc4 */
+
+    pream_nmea_got_sync1,         /**< NMEA Frame '$' */
+    pream_nmea_got_asteriks,      /**< NMEA Frame '*' */
+    pream_nmea_got_first_cs_byte, /**< NMEA Frame cs first byte */
+
+    decode_rtcm3                  /**< Frame rtcm3 */
 };
 
-class GPSDriverFemto : public GPSBaseStationSupport
-{
+class GPSDriverFemto : public GPSBaseStationSupport {
 public:
-	/**
+    /**
 	 * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement.
 	 */
-	GPSDriverFemto(GPSCallbackPtr callback, void *callback_user, struct sensor_gps_s *gps_position,
-		       satellite_info_s *satellite_info = nullptr,
-		       float heading_offset = 0.f);
-	virtual ~GPSDriverFemto();
+    GPSDriverFemto(GPSCallbackPtr callback, void *callback_user, struct sensor_gps_s *gps_position,
+                   satellite_info_s *satellite_info = nullptr,
+                   float             heading_offset = 0.f);
+    virtual ~GPSDriverFemto();
 
-	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int receive(unsigned timeout) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
-
-	/**
+    /**
 	 * when Constructor is work, initialize parameters
 	 */
-	void decodeInit(void);
+    void decodeInit(void);
 
-	/**
+    /**
 	 * check the message if whether is 8001,memcpy data to _gps_position
 	 */
-	int handleMessage(int len);
+    int handleMessage(int len);
 
-	/**
+    /**
 	 * analysis frame data from buf[] to _femto_msg and check the frame is suceess or not
 	 */
-	int parseChar(uint8_t b);
+    int parseChar(uint8_t b);
 
-	/**
+    /**
 	 * Write a command and wait for a (N)Ack
 	 * @return 0 on success, <0 otherwise
 	 */
-	int writeAckedCommandFemto(const char *command, const char *reply, const unsigned timeout);
+    int writeAckedCommandFemto(const char *command, const char *reply, const unsigned timeout);
 
-	/**
+    /**
 	 * receive data for at least the specified amount of time
 	 */
-	void receiveWait(unsigned timeout_min);
+    void receiveWait(unsigned timeout_min);
 
-	/**
+    /**
 	* enable output of correction output
 	*/
-	void activateCorrectionOutput();
+    void activateCorrectionOutput();
 
-	/**
+    /**
 	 * enable output of rtcm
 	 */
-	void activateRTCMOutput();
+    void activateRTCMOutput();
 
-	/**
+    /**
 	 * update survery in status of QGC RTK GPS
 	 */
-	void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = (double)NAN,
-				      double longitude = (double)NAN, float altitude = NAN);
+    void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = (double)NAN,
+                                  double longitude = (double)NAN, float altitude = NAN);
 
 
-	struct sensor_gps_s 	*_gps_position {nullptr};
-	FemtoDecodeState		_decode_state{FemtoDecodeState::pream_ble1};
-	femto_uav_gps_t			_femto_uav_gps;
-	femto_msg_t 			_femto_msg;
-	satellite_info_s        *_satellite_info{nullptr};
-	float 					_heading_offset;
+    struct sensor_gps_s *_gps_position{nullptr};
+    FemtoDecodeState     _decode_state{FemtoDecodeState::pream_ble1};
+    femto_uav_gps_t      _femto_uav_gps;
+    femto_msg_t          _femto_msg;
+    satellite_info_s    *_satellite_info{nullptr};
+    float                _heading_offset;
 
-	RTCMParsing             *_rtcm_parsing{nullptr};
-	OutputMode              _output_mode{OutputMode::GPS};
-	bool                    _configure_done{false};
-	bool                    _correction_output_activated{false};
+    RTCMParsing *_rtcm_parsing{nullptr};
+    OutputMode   _output_mode{OutputMode::GPS};
+    bool         _configure_done{false};
+    bool         _correction_output_activated{false};
 
-	gps_abstime 			_survey_in_start{0};
+    gps_abstime _survey_in_start{0};
 };
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.cpp
index eaa687b2f2..eb74f7bc99 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.cpp
@@ -1,41 +1,18 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "gps_helper.h"
 #include <math.h>
 
 #ifndef M_PI
-#define M_PI		3.141592653589793238462643383280
+#   define M_PI 3.141592653589793238462643383280
 #endif
 
 
@@ -47,57 +24,51 @@
  */
 
 GPSHelper::GPSHelper(GPSCallbackPtr callback, void *callback_user) :
-	_callback(callback),
-	_callback_user(callback_user)
-{
+    _callback(callback),
+    _callback_user(callback_user) {
 }
 
-void
-GPSHelper::resetUpdateRates()
-{
-	_rate_count_vel = 0;
-	_rate_count_lat_lon = 0;
-	_interval_rate_start = gps_absolute_time();
+void GPSHelper::resetUpdateRates() {
+    _rate_count_vel      = 0;
+    _rate_count_lat_lon  = 0;
+    _interval_rate_start = gps_absolute_time();
 }
 
-void
-GPSHelper::storeUpdateRates()
-{
-	_rate_vel = _rate_count_vel / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f);
-	_rate_lat_lon = _rate_count_lat_lon / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f);
+void GPSHelper::storeUpdateRates() {
+    _rate_vel     = _rate_count_vel / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f);
+    _rate_lat_lon = _rate_count_lat_lon / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f);
 }
 
 void GPSHelper::ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude,
-			 float &altitude)
-{
-	// WGS84 ellipsoid constants
-	constexpr double a = 6378137.; // radius
-	constexpr double e = 8.1819190842622e-2;  // eccentricity
+                         float &altitude) {
+    // WGS84 ellipsoid constants
+    constexpr double a = 6378137.;           // radius
+    constexpr double e = 8.1819190842622e-2; // eccentricity
 
-	constexpr double asq = a * a;
-	constexpr double esq = e * e;
+    constexpr double asq = a * a;
+    constexpr double esq = e * e;
 
-	double x = ecef_x;
-	double y = ecef_y;
-	double z = ecef_z;
+    double x = ecef_x;
+    double y = ecef_y;
+    double z = ecef_z;
 
-	double b = sqrt(asq * (1. - esq));
-	double bsq = b * b;
-	double ep = sqrt((asq - bsq) / bsq);
-	double p = sqrt(x * x + y * y);
-	double th = atan2(a * z, b * p);
+    double b   = sqrt(asq * (1. - esq));
+    double bsq = b * b;
+    double ep  = sqrt((asq - bsq) / bsq);
+    double p   = sqrt(x * x + y * y);
+    double th  = atan2(a * z, b * p);
 
-	longitude = atan2(y, x);
-	double sin_th = sin(th);
-	double cos_th = cos(th);
-	latitude = atan2(z + ep * ep * b * sin_th * sin_th * sin_th, p - esq * a * cos_th * cos_th * cos_th);
-	double sin_lat = sin(latitude);
-	double N = a / sqrt(1. - esq * sin_lat * sin_lat);
-	altitude = (float)(p / cos(latitude) - N);
+    longitude      = atan2(y, x);
+    double sin_th  = sin(th);
+    double cos_th  = cos(th);
+    latitude       = atan2(z + ep * ep * b * sin_th * sin_th * sin_th, p - esq * a * cos_th * cos_th * cos_th);
+    double sin_lat = sin(latitude);
+    double N       = a / sqrt(1. - esq * sin_lat * sin_lat);
+    altitude       = (float)(p / cos(latitude) - N);
 
-	// rad to deg
-	longitude *= 180. / M_PI;
-	latitude *= 180. / M_PI;
+    // rad to deg
+    longitude *= 180. / M_PI;
+    latitude  *= 180. / M_PI;
 
-	// correction for altitude near poles left out.
+    // correction for altitude near poles left out.
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.h b/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.h
index f7301f0108..338a6eab0e 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/gps_helper.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2014 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file gps_helper.h
@@ -44,15 +21,15 @@
 #include "../../definitions.h"
 
 #ifndef GPS_READ_BUFFER_SIZE
-#define GPS_READ_BUFFER_SIZE 150 ///< buffer size for the read() call. Messages can be longer than that.
+#   define GPS_READ_BUFFER_SIZE 150 ///< buffer size for the read() call. Messages can be longer than that.
 #endif
 
 #ifndef M_PI_F
-# define M_PI_F 3.14159265358979323846f
+#   define M_PI_F 3.14159265358979323846f
 #endif
 
 enum class GPSCallbackType {
-	/**
+    /**
 	 * Read data from device. This is a blocking operation with a timeout.
 	 * data1: points to a buffer to be written to. The first sizeof(int) bytes contain the
 	 *        timeout in ms when calling the method.
@@ -60,82 +37,82 @@ enum class GPSCallbackType {
 	 * return: num read bytes, 0 on timeout (the method can actually also return 0 before
 	 *         the timeout happens).
 	 */
-	readDeviceData = 0,
+    readDeviceData = 0,
 
-	/**
+    /**
 	 * Write data to device
 	 * data1: data to be written
 	 * data2: number of bytes to write
 	 * return: num written bytes
 	 */
-	writeDeviceData,
+    writeDeviceData,
 
-	/**
+    /**
 	 * set Baudrate
 	 * data1: ignored
 	 * data2: baudrate
 	 * return: 0 on success
 	 */
-	setBaudrate,
+    setBaudrate,
 
-	/**
+    /**
 	 * Got an RTCM message from the device.
 	 * data1: pointer to the message
 	 * data2: message length
 	 * return: ignored
 	 */
-	gotRTCMMessage,
+    gotRTCMMessage,
 
-	/**
+    /**
 	 * Got a relative position message from the device.
 	 * data1: pointer to the message
 	 * data2: message length
 	 * return: ignored
 	 */
-	gotRelativePositionMessage,
+    gotRelativePositionMessage,
 
-	/**
+    /**
 	 * message about current survey-in status
 	 * data1: points to a SurveyInStatus struct
 	 * data2: ignored
 	 * return: ignored
 	 */
-	surveyInStatus,
+    surveyInStatus,
 
-	/**
+    /**
 	 * can be used to set the current clock accurately
 	 * data1: pointer to a timespec struct
 	 * data2: ignored
 	 * return: ignored
 	 */
-	setClock,
+    setClock,
 };
 
 enum class GPSRestartType {
-	None,
+    None,
 
-	/**
+    /**
 	 * In hot start mode, the receiver was powered down only for a short time (4 hours or less),
 	 * so that its ephemeris is still valid. Since the receiver doesn't need to download ephemeris
 	 * again, this is the fastest startup method.
 	 */
-	Hot,
+    Hot,
 
-	/**
+    /**
 	 * In warm start mode, the receiver has approximate information for time, position, and coarse
 	 * satellite position data (Almanac). In this mode, after power-up, the receiver normally needs
 	 * to download ephemeris before it can calculate position and velocity data.
 	 */
-	Warm,
+    Warm,
 
-	/**
+    /**
 	 * In cold start mode, the receiver has no information from the last position at startup.
 	 * Therefore, the receiver must search the full time and frequency space, and all possible
 	 * satellite numbers. If a satellite signal is found, it is tracked to decode the ephemeris,
 	 * whereas the other channels continue to search satellites. Once there is a sufficient number
 	 * of satellites with valid ephemeris, the receiver can calculate position and velocity data.
 	 */
-	Cold
+    Cold
 };
 
 /** Callback function for platform-specific stuff.
@@ -144,106 +121,113 @@ enum class GPSRestartType {
  */
 typedef int (*GPSCallbackPtr)(GPSCallbackType type, void *data1, int data2, void *user);
 
-
 struct SurveyInStatus {
-	double latitude;              /**< NAN if unknown/not set [deg] */
-	double longitude;             /**< NAN if unknown/not set [deg] */
-	float altitude;               /**< NAN if unknown/not set [m] */
-	uint32_t mean_accuracy;       /**< [mm] */
-	uint32_t duration;            /**< [s] */
-	uint8_t flags;                /**< bit 0: valid, bit 1: active */
+    double   latitude;      /**< NAN if unknown/not set [deg] */
+    double   longitude;     /**< NAN if unknown/not set [deg] */
+    float    altitude;      /**< NAN if unknown/not set [m] */
+    uint32_t mean_accuracy; /**< [mm] */
+    uint32_t duration;      /**< [s] */
+    uint8_t  flags;         /**< bit 0: valid, bit 1: active */
 };
 
 // TODO: this number seems wrong
 #define GPS_EPOCH_SECS ((time_t)1234567890ULL)
 
-class GPSHelper
-{
+class GPSHelper {
 public:
-	enum class OutputMode : uint8_t {
-		GPS = 0,    ///< normal GPS output
-		GPSAndRTCM, ///< normal GPS+RTCM output
-		RTCM        ///< request RTCM output. This is used for (fixed position) base stations
-	};
-
-	enum class Interface : uint8_t {
-		UART = 0,
-		SPI
-	};
-
-	/**
+    enum class OutputMode : uint8_t {
+        GPS = 0,    ///< normal GPS output
+        GPSAndRTCM, ///< normal GPS+RTCM output
+        RTCM        ///< request RTCM output. This is used for (fixed position) base stations
+    };
+
+    enum class Interface : uint8_t {
+        UART = 0,
+        SPI
+    };
+
+    /**
 	 * Bitmask for GPS_1_GNSS and GPS_2_GNSS
 	 * No bits set should keep the receiver's default config
 	 */
-	enum class GNSSSystemsMask : int32_t {
-		RECEIVER_DEFAULTS = 0,
-		ENABLE_GPS =        1 << 0,
-		ENABLE_SBAS =       1 << 1,
-		ENABLE_GALILEO =    1 << 2,
-		ENABLE_BEIDOU =     1 << 3,
-		ENABLE_GLONASS =    1 << 4
-	};
-
-	enum class InterfaceProtocolsMask : int32_t {
-		ALL_DISABLED =        0,
-		I2C_IN_PROT_UBX =     1 << 0,
-		I2C_IN_PROT_NMEA =    1 << 1,
-		I2C_IN_PROT_RTCM3X =  1 << 2,
-		I2C_OUT_PROT_UBX =    1 << 3,
-		I2C_OUT_PROT_NMEA =   1 << 4,
-		I2C_OUT_PROT_RTCM3X = 1 << 5
-	};
-
-	struct GPSConfig {
-		OutputMode output_mode;
-		GNSSSystemsMask gnss_systems;
-		InterfaceProtocolsMask interface_protocols;
-	};
-
-
-	GPSHelper(GPSCallbackPtr callback, void *callback_user);
-	virtual ~GPSHelper() = default;
-
-	/**
+    enum class GNSSSystemsMask : int32_t {
+        RECEIVER_DEFAULTS = 0,
+        ENABLE_GPS        = 1 << 0,
+        ENABLE_SBAS       = 1 << 1,
+        ENABLE_GALILEO    = 1 << 2,
+        ENABLE_BEIDOU     = 1 << 3,
+        ENABLE_GLONASS    = 1 << 4
+    };
+
+    enum class InterfaceProtocolsMask : int32_t {
+        ALL_DISABLED        = 0,
+        I2C_IN_PROT_UBX     = 1 << 0,
+        I2C_IN_PROT_NMEA    = 1 << 1,
+        I2C_IN_PROT_RTCM3X  = 1 << 2,
+        I2C_OUT_PROT_UBX    = 1 << 3,
+        I2C_OUT_PROT_NMEA   = 1 << 4,
+        I2C_OUT_PROT_RTCM3X = 1 << 5
+    };
+
+    struct GPSConfig {
+        OutputMode             output_mode;
+        GNSSSystemsMask        gnss_systems;
+        InterfaceProtocolsMask interface_protocols;
+    };
+
+    GPSHelper(GPSCallbackPtr callback, void *callback_user);
+    virtual ~GPSHelper() = default;
+
+    /**
 	 * configure the device
 	 * @param baud Input and output parameter: if set to 0, the baudrate will be automatically detected and set to
 	 *             the detected baudrate. If not 0, a fixed baudrate is used.
 	 * @param config GPS Config
 	 * @return 0 on success, <0 otherwise
 	 */
-	virtual int configure(unsigned &baud, const GPSConfig &config) = 0;
+    virtual int configure(unsigned &baud, const GPSConfig &config) = 0;
 
-	/**
+    /**
 	 * receive & handle new data from the device
 	 * @param timeout [ms]
 	 * @return <0 on error, otherwise a bitset:
 	 *         bit 0 set: got gps position update
 	 *         bit 1 set: got satellite info update
 	 */
-	virtual int receive(unsigned timeout) = 0;
+    virtual int receive(unsigned timeout) = 0;
 
-	/**
+    /**
 	 * Reset GPS device
 	 * @param restart_type
 	 * @return <0 failure
 	 *         -1 not implemented
 	 * 	    0 success
 	 */
-	virtual int reset(GPSRestartType restart_type)	{ (void)restart_type; return -1; }
+    virtual int reset(GPSRestartType restart_type) {
+        (void)restart_type;
+        return -1;
+    }
+
+    float getPositionUpdateRate() {
+        return _rate_lat_lon;
+    }
+
+    float getVelocityUpdateRate() {
+        return _rate_vel;
+    }
 
-	float getPositionUpdateRate() { return _rate_lat_lon; }
-	float getVelocityUpdateRate() { return _rate_vel; }
-	void resetUpdateRates();
-	void storeUpdateRates();
+    void resetUpdateRates();
+    void storeUpdateRates();
 
-	/**
+    /**
 	 * Allow a driver to disable RTCM injection
 	 */
-	virtual bool shouldInjectRTCM() { return true; }
+    virtual bool shouldInjectRTCM() {
+        return true;
+    }
 
 protected:
-
-	/**
+    /**
 	 * read from device
 	 * @param buf: pointer to read buffer
 	 * @param buf_length: size of read buffer
@@ -252,56 +236,49 @@ class GPSHelper
 	 *	    < 0 for error
 	 *	    > 0 number of bytes read
 	 */
-	int read(uint8_t *buf, int buf_length, int timeout)
-	{
-		memcpy(buf, &timeout, sizeof(timeout));
-		return _callback(GPSCallbackType::readDeviceData, buf, buf_length, _callback_user);
-	}
+    int read(uint8_t *buf, int buf_length, int timeout) {
+        memcpy(buf, &timeout, sizeof(timeout));
+        return _callback(GPSCallbackType::readDeviceData, buf, buf_length, _callback_user);
+    }
 
-	/**
+    /**
 	 * write to the device
 	 * @param buf
 	 * @param buf_length
 	 * @return num written bytes, -1 on error
 	 */
-	int write(const void *buf, int buf_length)
-	{
-		return _callback(GPSCallbackType::writeDeviceData, (void *)buf, buf_length, _callback_user);
-	}
+    int write(const void *buf, int buf_length) {
+        return _callback(GPSCallbackType::writeDeviceData, (void *)buf, buf_length, _callback_user);
+    }
 
-	/**
+    /**
 	 * set the Baudrate
 	 * @param baudrate
 	 * @return 0 on success, <0 otherwise
 	 */
-	int setBaudrate(int baudrate)
-	{
-		return _callback(GPSCallbackType::setBaudrate, nullptr, baudrate, _callback_user);
-	}
-
-	void surveyInStatus(SurveyInStatus &status)
-	{
-		_callback(GPSCallbackType::surveyInStatus, &status, 0, _callback_user);
-	}
-
-	/** got an RTCM message from the device */
-	void gotRTCMMessage(uint8_t *buf, int buf_length)
-	{
-		_callback(GPSCallbackType::gotRTCMMessage, buf, buf_length, _callback_user);
-	}
-
-	/** got a relative position message from the device */
-	void gotRelativePositionMessage(sensor_gnss_relative_s &gnss_relative)
-	{
-		_callback(GPSCallbackType::gotRelativePositionMessage, &gnss_relative, sizeof(sensor_gnss_relative_s), _callback_user);
-	}
-
-	void setClock(timespec &t)
-	{
-		_callback(GPSCallbackType::setClock, &t, 0, _callback_user);
-	}
-
-	/**
+    int setBaudrate(int baudrate) {
+        return _callback(GPSCallbackType::setBaudrate, nullptr, baudrate, _callback_user);
+    }
+
+    void surveyInStatus(SurveyInStatus &status) {
+        _callback(GPSCallbackType::surveyInStatus, &status, 0, _callback_user);
+    }
+
+    /** got an RTCM message from the device */
+    void gotRTCMMessage(uint8_t *buf, int buf_length) {
+        _callback(GPSCallbackType::gotRTCMMessage, buf, buf_length, _callback_user);
+    }
+
+    /** got a relative position message from the device */
+    void gotRelativePositionMessage(sensor_gnss_relative_s &gnss_relative) {
+        _callback(GPSCallbackType::gotRelativePositionMessage, &gnss_relative, sizeof(sensor_gnss_relative_s), _callback_user);
+    }
+
+    void setClock(timespec &t) {
+        _callback(GPSCallbackType::setClock, &t, 0, _callback_user);
+    }
+
+    /**
 	 * Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt).
 	 * Ported from: https://stackoverflow.com/a/25428344
 	 * @param ecef_x ECEF X-coordinate [m]
@@ -311,26 +288,24 @@ class GPSHelper
 	 * @param longitude [deg]
 	 * @param altitude [m]
 	 */
-	static void ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, float &altitude);
+    static void ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, float &altitude);
 
-	GPSCallbackPtr _callback{nullptr};
-	void *_callback_user{};
+    GPSCallbackPtr _callback{nullptr};
+    void          *_callback_user{};
 
-	uint8_t _rate_count_lat_lon{};
-	uint8_t _rate_count_vel{};
+    uint8_t _rate_count_lat_lon{};
+    uint8_t _rate_count_vel{};
 
-	float _rate_lat_lon{0.0f};
-	float _rate_vel{0.0f};
+    float _rate_lat_lon{0.0f};
+    float _rate_vel{0.0f};
 
-	uint64_t _interval_rate_start{0};
+    uint64_t _interval_rate_start{0};
 };
 
-inline bool operator&(GPSHelper::GNSSSystemsMask a, GPSHelper::GNSSSystemsMask b)
-{
-	return static_cast<int32_t>(a) & static_cast<int32_t>(b);
+inline bool operator&(GPSHelper::GNSSSystemsMask a, GPSHelper::GNSSSystemsMask b) {
+    return static_cast<int32_t>(a) & static_cast<int32_t>(b);
 }
 
-inline bool operator&(GPSHelper::InterfaceProtocolsMask a, GPSHelper::InterfaceProtocolsMask b)
-{
-	return static_cast<int32_t>(a) & static_cast<int32_t>(b);
+inline bool operator&(GPSHelper::InterfaceProtocolsMask a, GPSHelper::InterfaceProtocolsMask b) {
+    return static_cast<int32_t>(a) & static_cast<int32_t>(b);
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/mtk.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/mtk.cpp
index 11ac27ed6c..7755b7a546 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/mtk.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/mtk.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2015 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file mtk.cpp
@@ -46,261 +23,246 @@
 #include <ctime>
 #include <math.h>
 
-
 GPSDriverMTK::GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position) :
-	GPSHelper(callback, callback_user),
-	_gps_position(gps_position)
-{
-	decodeInit();
+    GPSHelper(callback, callback_user),
+    _gps_position(gps_position) {
+    decodeInit();
 }
 
-int
-GPSDriverMTK::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	if (config.output_mode != OutputMode::GPS) {
-		GPS_WARN("MTK: Unsupported Output Mode %i", (int)config.output_mode);
-		return -1;
-	}
+int GPSDriverMTK::configure(unsigned &baudrate, const GPSConfig &config) {
+    if (config.output_mode != OutputMode::GPS) {
+        GPS_WARN("MTK: Unsupported Output Mode %i", (int)config.output_mode);
+        return -1;
+    }
 
-	if (baudrate > 0 && baudrate != MTK_BAUDRATE) {
-		return -1;
-	}
+    if (baudrate > 0 && baudrate != MTK_BAUDRATE) {
+        return -1;
+    }
 
-	/* set baudrate first */
-	if (GPSHelper::setBaudrate(MTK_BAUDRATE) != 0) {
-		return -1;
-	}
+    /* set baudrate first */
+    if (GPSHelper::setBaudrate(MTK_BAUDRATE) != 0) {
+        return -1;
+    }
 
-	baudrate = MTK_BAUDRATE;
+    baudrate = MTK_BAUDRATE;
 
-	/* Write config messages, don't wait for an answer */
-	if (strlen(MTK_OUTPUT_5HZ) != write(MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
-		goto errout;
-	}
+    /* Write config messages, don't wait for an answer */
+    if (strlen(MTK_OUTPUT_5HZ) != write(MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+        goto errout;
+    }
 
-	gps_usleep(10000);
+    gps_usleep(10000);
 
-	if (strlen(MTK_SET_BINARY) != write(MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
-		goto errout;
-	}
+    if (strlen(MTK_SET_BINARY) != write(MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+        goto errout;
+    }
 
-	gps_usleep(10000);
+    gps_usleep(10000);
 
-	if (strlen(MTK_SBAS_ON) != write(MTK_SBAS_ON, strlen(MTK_SBAS_ON))) {
-		goto errout;
-	}
+    if (strlen(MTK_SBAS_ON) != write(MTK_SBAS_ON, strlen(MTK_SBAS_ON))) {
+        goto errout;
+    }
 
-	gps_usleep(10000);
+    gps_usleep(10000);
 
-	if (strlen(MTK_WAAS_ON) != write(MTK_WAAS_ON, strlen(MTK_WAAS_ON))) {
-		goto errout;
-	}
+    if (strlen(MTK_WAAS_ON) != write(MTK_WAAS_ON, strlen(MTK_WAAS_ON))) {
+        goto errout;
+    }
 
-	gps_usleep(10000);
+    gps_usleep(10000);
 
-	if (strlen(MTK_NAVTHRES_OFF) != write(MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
-		goto errout;
-	}
+    if (strlen(MTK_NAVTHRES_OFF) != write(MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+        goto errout;
+    }
 
-	return 0;
+    return 0;
 
 errout:
-	GPS_WARN("mtk: config write failed");
-	return -1;
+    GPS_WARN("mtk: config write failed");
+    return -1;
 }
 
-int
-GPSDriverMTK::receive(unsigned timeout)
-{
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
-	gps_mtk_packet_t packet{};
-
-	/* timeout additional to poll */
-	gps_abstime time_started = gps_absolute_time();
+int GPSDriverMTK::receive(unsigned timeout) {
+    uint8_t          buf[GPS_READ_BUFFER_SIZE];
+    gps_mtk_packet_t packet{};
 
-	int j = 0;
+    /* timeout additional to poll */
+    gps_abstime time_started = gps_absolute_time();
 
-	while (true) {
+    int j = 0;
 
-		int ret = read(buf, sizeof(buf), timeout);
+    while (true) {
+        int ret = read(buf, sizeof(buf), timeout);
 
-		if (ret > 0) {
-			/* first read whatever is left */
-			if (j < ret) {
-				/* pass received bytes to the packet decoder */
-				while (j < ret) {
-					if (parseChar(buf[j], packet) > 0) {
-						handleMessage(packet);
-						return 1;
-					}
+        if (ret > 0) {
+            /* first read whatever is left */
+            if (j < ret) {
+                /* pass received bytes to the packet decoder */
+                while (j < ret) {
+                    if (parseChar(buf[j], packet) > 0) {
+                        handleMessage(packet);
+                        return 1;
+                    }
 
-					j++;
-				}
+                    j++;
+                }
 
-				/* everything is read */
-				j = 0;
-			}
+                /* everything is read */
+                j = 0;
+            }
 
-		} else {
-			gps_usleep(20000);
-		}
+        } else {
+            gps_usleep(20000);
+        }
 
-		/* in case we keep trying but only get crap from GPS */
-		if (time_started + timeout * 1000 < gps_absolute_time()) {
-			return -1;
-		}
-	}
+        /* in case we keep trying but only get crap from GPS */
+        if (time_started + timeout * 1000 < gps_absolute_time()) {
+            return -1;
+        }
+    }
 }
 
-void
-GPSDriverMTK::decodeInit()
-{
-	_rx_ck_a = 0;
-	_rx_ck_b = 0;
-	_rx_count = 0;
-	_decode_state = MTK_DECODE_UNINIT;
+void GPSDriverMTK::decodeInit() {
+    _rx_ck_a      = 0;
+    _rx_ck_b      = 0;
+    _rx_count     = 0;
+    _decode_state = MTK_DECODE_UNINIT;
 }
-int
-GPSDriverMTK::parseChar(uint8_t b, gps_mtk_packet_t &packet)
-{
-	int ret = 0;
-
-	if (_decode_state == MTK_DECODE_UNINIT) {
-
-		if (b == MTK_SYNC1_V16) {
-			_decode_state = MTK_DECODE_GOT_CK_A;
-			_mtk_revision = 16;
-
-		} else if (b == MTK_SYNC1_V19) {
-			_decode_state = MTK_DECODE_GOT_CK_A;
-			_mtk_revision = 19;
-		}
-
-	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
-		if (b == MTK_SYNC2) {
-			_decode_state = MTK_DECODE_GOT_CK_B;
-
-		} else {
-			// Second start symbol was wrong, reset state machine
-			decodeInit();
-		}
-
-	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
-		// Add to checksum
-		if (_rx_count < 33) {
-			addByteToChecksum(b);
-		}
-
-		// Fill packet buffer
-		((uint8_t *)(&packet))[_rx_count] = b;
-		_rx_count++;
-
-		/* Packet size minus checksum, XXX ? */
-		if (_rx_count >= sizeof(packet)) {
-			/* Compare checksum */
-			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
-				ret = 1;
-
-			} else {
-				ret = -1;
-			}
-
-			// Reset state machine to decode next packet
-			decodeInit();
-		}
-	}
-
-	return ret;
+
+int GPSDriverMTK::parseChar(uint8_t b, gps_mtk_packet_t &packet) {
+    int ret = 0;
+
+    if (_decode_state == MTK_DECODE_UNINIT) {
+        if (b == MTK_SYNC1_V16) {
+            _decode_state = MTK_DECODE_GOT_CK_A;
+            _mtk_revision = 16;
+
+        } else if (b == MTK_SYNC1_V19) {
+            _decode_state = MTK_DECODE_GOT_CK_A;
+            _mtk_revision = 19;
+        }
+
+    } else if (_decode_state == MTK_DECODE_GOT_CK_A) {
+        if (b == MTK_SYNC2) {
+            _decode_state = MTK_DECODE_GOT_CK_B;
+
+        } else {
+            // Second start symbol was wrong, reset state machine
+            decodeInit();
+        }
+
+    } else if (_decode_state == MTK_DECODE_GOT_CK_B) {
+        // Add to checksum
+        if (_rx_count < 33) {
+            addByteToChecksum(b);
+        }
+
+        // Fill packet buffer
+        ((uint8_t *)(&packet))[_rx_count] = b;
+        _rx_count++;
+
+        /* Packet size minus checksum, XXX ? */
+        if (_rx_count >= sizeof(packet)) {
+            /* Compare checksum */
+            if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
+                ret = 1;
+
+            } else {
+                ret = -1;
+            }
+
+            // Reset state machine to decode next packet
+            decodeInit();
+        }
+    }
+
+    return ret;
 }
 
-void
-GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet)
-{
-	if (_mtk_revision == 16) {
-		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
-		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
-
-	} else if (_mtk_revision == 19) {
-		_gps_position->lat = packet.latitude; // both degrees*1e7
-		_gps_position->lon = packet.longitude; // both degrees*1e7
-
-	} else {
-		GPS_WARN("mtk: unknown revision");
-		_gps_position->lat = 0;
-		_gps_position->lon = 0;
-
-		// Indicate this data is not usable and bail out
-		_gps_position->eph = 1000.0f;
-		_gps_position->epv = 1000.0f;
-		_gps_position->fix_type = 0;
-		return;
-	}
-
-	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
-	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph = packet.hdop / 100.0f; // from cm to m
-	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
-	_gps_position->hdop = packet.hdop / 100.0f;
-	_gps_position->vdop = _gps_position->hdop;
-	_gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s
-	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	_gps_position->satellites_used = packet.satellites;
-
-	/* convert time and date information to unix timestamp */
-	struct tm timeinfo = {};
-	uint32_t timeinfo_conversion_temp;
-
-	timeinfo.tm_mday = packet.date / 10000;
-	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000;
-	timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1;
-	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100;
-
-	timeinfo.tm_hour = (packet.utc_time / 10000000);
-	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000;
-	timeinfo.tm_min = timeinfo_conversion_temp / 100000;
-	timeinfo_conversion_temp -= timeinfo.tm_min * 100000;
-	timeinfo.tm_sec = timeinfo_conversion_temp / 1000;
-	timeinfo_conversion_temp -= timeinfo.tm_sec * 1000;
-
-	timeinfo.tm_isdst = 0;
+void GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet) {
+    if (_mtk_revision == 16) {
+        _gps_position->lat = packet.latitude * 10;  // from degrees*1e6 to degrees*1e7
+        _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
+    } else if (_mtk_revision == 19) {
+        _gps_position->lat = packet.latitude;  // both degrees*1e7
+        _gps_position->lon = packet.longitude; // both degrees*1e7
+
+    } else {
+        GPS_WARN("mtk: unknown revision");
+        _gps_position->lat = 0;
+        _gps_position->lon = 0;
+
+        // Indicate this data is not usable and bail out
+        _gps_position->eph      = 1000.0f;
+        _gps_position->epv      = 1000.0f;
+        _gps_position->fix_type = 0;
+        return;
+    }
+
+    _gps_position->alt             = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+    _gps_position->fix_type        = packet.fix_type;
+    _gps_position->eph             = packet.hdop / 100.0f;                // from cm to m
+    _gps_position->epv             = _gps_position->eph;                  // unknown in mtk custom mode, so we cheat with eph
+    _gps_position->hdop            = packet.hdop / 100.0f;
+    _gps_position->vdop            = _gps_position->hdop;
+    _gps_position->vel_m_s         = ((float)packet.ground_speed) / 100.0f;            // from cm/s to m/s
+    _gps_position->cog_rad         = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
+    _gps_position->satellites_used = packet.satellites;
+
+    /* convert time and date information to unix timestamp */
+    struct tm timeinfo = {};
+    uint32_t  timeinfo_conversion_temp;
+
+    timeinfo.tm_mday         = packet.date / 10000;
+    timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000;
+    timeinfo.tm_mon          = (timeinfo_conversion_temp / 100) - 1;
+    timeinfo.tm_year         = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100;
+
+    timeinfo.tm_hour          = (packet.utc_time / 10000000);
+    timeinfo_conversion_temp  = packet.utc_time - timeinfo.tm_hour * 10000000;
+    timeinfo.tm_min           = timeinfo_conversion_temp / 100000;
+    timeinfo_conversion_temp -= timeinfo.tm_min * 100000;
+    timeinfo.tm_sec           = timeinfo_conversion_temp / 1000;
+    timeinfo_conversion_temp -= timeinfo.tm_sec * 1000;
+
+    timeinfo.tm_isdst = 0;
 
 #ifndef NO_MKTIME
 
-	time_t epoch = mktime(&timeinfo);
+    time_t epoch = mktime(&timeinfo);
 
-	if (epoch > GPS_EPOCH_SECS) {
-		// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-		// and control its drift. Since we rely on the HRT for our monotonic
-		// clock, updating it from time to time is safe.
+    if (epoch > GPS_EPOCH_SECS) {
+        // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+        // and control its drift. Since we rely on the HRT for our monotonic
+        // clock, updating it from time to time is safe.
 
-		timespec ts{};
-		ts.tv_sec = epoch;
-		ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
+        timespec ts{};
+        ts.tv_sec  = epoch;
+        ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
 
-		setClock(ts);
+        setClock(ts);
 
-		_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-		_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
+        _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+        _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
 
-	} else {
-		_gps_position->time_utc_usec = 0;
-	}
+    } else {
+        _gps_position->time_utc_usec = 0;
+    }
 
 #else
-	_gps_position->time_utc_usec = 0;
+    _gps_position->time_utc_usec = 0;
 #endif
 
-	_gps_position->timestamp = gps_absolute_time();
-	_gps_position->timestamp_time_relative = 0;
+    _gps_position->timestamp               = gps_absolute_time();
+    _gps_position->timestamp_time_relative = 0;
 
-	// Position and velocity update always at the same time
-	_rate_count_vel++;
-	_rate_count_lat_lon++;
+    // Position and velocity update always at the same time
+    _rate_count_vel++;
+    _rate_count_lat_lon++;
 }
 
-void
-GPSDriverMTK::addByteToChecksum(uint8_t b)
-{
-	_rx_ck_a = _rx_ck_a + b;
-	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+void GPSDriverMTK::addByteToChecksum(uint8_t b) {
+    _rx_ck_a = _rx_ck_a + b;
+    _rx_ck_b = _rx_ck_b + _rx_ck_a;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/mtk.h b/apps/peripheral/sensor/gnss/gps/devices/src/mtk.h
index 3302f081b6..78838f7679 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/mtk.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/mtk.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2016 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file mtk.h
@@ -43,82 +20,81 @@
 #include "gps_helper.h"
 #include "../../definitions.h"
 
-#define MTK_SYNC1_V16 0xd0
-#define MTK_SYNC1_V19 0xd1
-#define MTK_SYNC2 0xdd
+#define MTK_SYNC1_V16    0xd0
+#define MTK_SYNC1_V19    0xd1
+#define MTK_SYNC2        0xdd
 
-#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
-#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
-#define MTK_SBAS_ON	       	"$PMTK313,1*2E\r\n"
-#define MTK_WAAS_ON		"$PMTK301,2*2E\r\n"
-#define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
+#define MTK_OUTPUT_5HZ   "$PMTK220,200*2C\r\n"
+#define MTK_SET_BINARY   "$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define MTK_SBAS_ON      "$PMTK313,1*2E\r\n"
+#define MTK_WAAS_ON      "$PMTK301,2*2E\r\n"
+#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n"
 
-#define MTK_TIMEOUT_5HZ 400
-#define MTK_BAUDRATE 38400
+#define MTK_TIMEOUT_5HZ  400
+#define MTK_BAUDRATE     38400
 
 typedef enum {
-	MTK_DECODE_UNINIT = 0,
-	MTK_DECODE_GOT_CK_A = 1,
-	MTK_DECODE_GOT_CK_B = 2
+    MTK_DECODE_UNINIT   = 0,
+    MTK_DECODE_GOT_CK_A = 1,
+    MTK_DECODE_GOT_CK_B = 2
 } mtk_decode_state_t;
 
 /** the structures of the binary packets */
 #pragma pack(push, 1)
 
 typedef struct {
-	uint8_t payload; ///< Number of payload bytes
-	int32_t latitude;  ///< Latitude in degrees * 10^7
-	int32_t longitude; ///< Longitude in degrees * 10^7
-	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
-	uint32_t ground_speed; ///< velocity in m/s
-	int32_t heading; ///< heading in degrees * 10^2
-	uint8_t satellites; ///< number of satellites used
-	uint8_t fix_type;  ///< fix type: XXX correct for that
-	uint32_t date;
-	uint32_t utc_time;
-	uint16_t hdop; ///< horizontal dilution of position (without unit)
-	uint8_t ck_a;
-	uint8_t ck_b;
+    uint8_t  payload;      ///< Number of payload bytes
+    int32_t  latitude;     ///< Latitude in degrees * 10^7
+    int32_t  longitude;    ///< Longitude in degrees * 10^7
+    uint32_t msl_altitude; ///< MSL altitude in meters * 10^2
+    uint32_t ground_speed; ///< velocity in m/s
+    int32_t  heading;      ///< heading in degrees * 10^2
+    uint8_t  satellites;   ///< number of satellites used
+    uint8_t  fix_type;     ///< fix type: XXX correct for that
+    uint32_t date;
+    uint32_t utc_time;
+    uint16_t hdop; ///< horizontal dilution of position (without unit)
+    uint8_t  ck_a;
+    uint8_t  ck_b;
 } gps_mtk_packet_t;
 
 #pragma pack(pop)
 
 #define MTK_RECV_BUFFER_SIZE 40
 
-class GPSDriverMTK : public GPSHelper
-{
+class GPSDriverMTK : public GPSHelper {
 public:
-	GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position);
-	virtual ~GPSDriverMTK() = default;
+    GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position);
+    virtual ~GPSDriverMTK() = default;
 
-	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int receive(unsigned timeout) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
-	/**
+    /**
 	 * Parse the binary MTK packet
 	 */
-	int parseChar(uint8_t b, gps_mtk_packet_t &packet);
+    int parseChar(uint8_t b, gps_mtk_packet_t &packet);
 
-	/**
+    /**
 	 * Handle the package once it has arrived
 	 */
-	void handleMessage(gps_mtk_packet_t &packet);
+    void handleMessage(gps_mtk_packet_t &packet);
 
-	/**
+    /**
 	 * Reset the parse state machine for a fresh start
 	 */
-	void decodeInit();
+    void decodeInit();
 
-	/**
+    /**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void addByteToChecksum(uint8_t);
-
-	sensor_gps_s *_gps_position {nullptr};
-	mtk_decode_state_t _decode_state{MTK_DECODE_UNINIT};
-	uint8_t _mtk_revision{0};
-	unsigned _rx_count{};
-	uint8_t _rx_ck_a{};
-	uint8_t _rx_ck_b{};
+    void addByteToChecksum(uint8_t);
+
+    sensor_gps_s      *_gps_position{nullptr};
+    mtk_decode_state_t _decode_state{MTK_DECODE_UNINIT};
+    uint8_t            _mtk_revision{0};
+    unsigned           _rx_count{};
+    uint8_t            _rx_ck_a{};
+    uint8_t            _rx_ck_b{};
 };
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/nmea.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/nmea.cpp
index 346a3516f2..eb7cc8cb3f 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/nmea.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/nmea.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020, 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file nmea.cpp
@@ -52,31 +29,32 @@
 #include "rtcm.h"
 
 #ifndef M_PI_F
-# define M_PI_F 3.14159265358979323846f
+#   define M_PI_F 3.14159265358979323846f
 #endif
 
-#define MAX(X,Y)    ((X) > (Y) ? (X) : (Y))
+#define MAX(X, Y)      ((X) > (Y) ? (X) : (Y))
 #define NMEA_UNUSED(x) (void)x;
 
 /**** Warning macros, disable to save memory */
-#define NMEA_WARN(...)         {GPS_WARN(__VA_ARGS__);}
-#define NMEA_DEBUG(...)        {/*GPS_WARN(__VA_ARGS__);*/}
+#define NMEA_WARN(...) \
+    { GPS_WARN(__VA_ARGS__); }
+#define NMEA_DEBUG(...)          \
+    { /*GPS_WARN(__VA_ARGS__);*/ \
+    }
 
 GPSDriverNMEA::GPSDriverNMEA(GPSCallbackPtr callback, void *callback_user,
-			     sensor_gps_s *gps_position,
-			     satellite_info_s *satellite_info,
-			     float heading_offset) :
-	GPSHelper(callback, callback_user),
-	_gps_position(gps_position),
-	_satellite_info(satellite_info),
-	_heading_offset(heading_offset)
-{
-	decodeInit();
+                             sensor_gps_s     *gps_position,
+                             satellite_info_s *satellite_info,
+                             float             heading_offset) :
+    GPSHelper(callback, callback_user),
+    _gps_position(gps_position),
+    _satellite_info(satellite_info),
+    _heading_offset(heading_offset) {
+    decodeInit();
 }
 
-GPSDriverNMEA::~GPSDriverNMEA()
-{
-	delete _rtcm_parsing;
+GPSDriverNMEA::~GPSDriverNMEA() {
+    delete _rtcm_parsing;
 }
 
 /*
@@ -84,26 +62,26 @@ GPSDriverNMEA::~GPSDriverNMEA()
  * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
  */
 
-int GPSDriverNMEA::handleMessage(int len)
-{
-	char *endp;
-
-	if (len < 7) {
-		return 0;
-	}
+int GPSDriverNMEA::handleMessage(int len) {
+    char *endp;
 
-	int uiCalcComma = 0;
+    if (len < 7) {
+        return 0;
+    }
 
-	for (int i = 0 ; i < len; i++) {
-		if (_rx_buffer[i] == ',') { uiCalcComma++; }
-	}
+    int uiCalcComma = 0;
 
-	char *bufptr = (char *)(_rx_buffer + 6);
-	int ret = 0;
+    for (int i = 0; i < len; i++) {
+        if (_rx_buffer[i] == ',') {
+            uiCalcComma++;
+        }
+    }
 
-	if ((memcmp(_rx_buffer + 3, "ZDA,", 4) == 0) && (uiCalcComma == 6)) {
+    char *bufptr = (char *)(_rx_buffer + 6);
+    int   ret    = 0;
 
-		/*
+    if ((memcmp(_rx_buffer + 3, "ZDA,", 4) == 0) && (uiCalcComma == 6)) {
+        /*
 		UTC day, month, and year, and local time zone offset
 		An example of the ZDA message string is:
 
@@ -121,72 +99,90 @@ int GPSDriverNMEA::handleMessage(int len)
 		7	The checksum data, always begins with *
 		Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
 		*/
-		double utc_time = 0.0;
-		int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
-		NMEA_UNUSED(local_time_off_hour);
-		NMEA_UNUSED(local_time_off_min);
-
-		if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		int utc_hour = static_cast<int>(utc_time / 10000);
-		int utc_minute = static_cast<int>((utc_time - utc_hour * 10000) / 100);
-		double utc_sec = static_cast<double>(utc_time - utc_hour * 10000 - utc_minute * 100);
-
-		/*
+        double utc_time = 0.0;
+        int    day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
+        NMEA_UNUSED(local_time_off_hour);
+        NMEA_UNUSED(local_time_off_min);
+
+        if (bufptr && *(++bufptr) != ',') {
+            utc_time = strtod(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            day    = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            month  = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            year   = strtol(bufptr, &endp, 10);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            local_time_off_hour = strtol(bufptr, &endp, 10);
+            bufptr              = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            local_time_off_min = strtol(bufptr, &endp, 10);
+            bufptr             = endp;
+        }
+
+        int    utc_hour   = static_cast<int>(utc_time / 10000);
+        int    utc_minute = static_cast<int>((utc_time - utc_hour * 10000) / 100);
+        double utc_sec    = static_cast<double>(utc_time - utc_hour * 10000 - utc_minute * 100);
+
+        /*
 		* convert to unix timestamp
 		*/
-		struct tm timeinfo = {};
-		timeinfo.tm_year = year - 1900;
-		timeinfo.tm_mon = month - 1;
-		timeinfo.tm_mday = day;
-		timeinfo.tm_hour = utc_hour;
-		timeinfo.tm_min = utc_minute;
-		timeinfo.tm_sec = int(utc_sec);
-		timeinfo.tm_isdst = 0;
+        struct tm timeinfo = {};
+        timeinfo.tm_year   = year - 1900;
+        timeinfo.tm_mon    = month - 1;
+        timeinfo.tm_mday   = day;
+        timeinfo.tm_hour   = utc_hour;
+        timeinfo.tm_min    = utc_minute;
+        timeinfo.tm_sec    = int(utc_sec);
+        timeinfo.tm_isdst  = 0;
 
 #ifndef NO_MKTIME
-		time_t epoch = mktime(&timeinfo);
+        time_t epoch = mktime(&timeinfo);
 
-		if (epoch > GPS_EPOCH_SECS) {
-			uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
+        if (epoch > GPS_EPOCH_SECS) {
+            uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
 
-			// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-			// and control its drift. Since we rely on the HRT for our monotonic
-			// clock, updating it from time to time is safe.
+            // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+            // and control its drift. Since we rely on the HRT for our monotonic
+            // clock, updating it from time to time is safe.
 
-			if (!_clock_set) {
-				timespec ts{};
-				ts.tv_sec = epoch;
-				ts.tv_nsec = usecs * 1000;
-				setClock(ts);
-				_clock_set = true;
-			}
+            if (!_clock_set) {
+                timespec ts{};
+                ts.tv_sec  = epoch;
+                ts.tv_nsec = usecs * 1000;
+                setClock(ts);
+                _clock_set = true;
+            }
 
-			_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-			_gps_position->time_utc_usec += usecs;
+            _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+            _gps_position->time_utc_usec += usecs;
 
-		} else {
-			_gps_position->time_utc_usec = 0;
-		}
+        } else {
+            _gps_position->time_utc_usec = 0;
+        }
 
 #else
-		_gps_position->time_utc_usec = 0;
+        _gps_position->time_utc_usec = 0;
 #endif
-		_TIME_received = true;
-		_gps_position->timestamp = gps_absolute_time();
+        _TIME_received           = true;
+        _gps_position->timestamp = gps_absolute_time();
 
-	} else if ((memcmp(_rx_buffer + 3, "GGA,", 4) == 0) && (uiCalcComma >= 14)) {
-		/*
+    } else if ((memcmp(_rx_buffer + 3, "GGA,", 4) == 0) && (uiCalcComma >= 14)) {
+        /*
 		  Time, position, and fix related data
 		  An example of the GBS message string is:
 		  $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs
@@ -223,87 +219,122 @@ int GPSDriverNMEA::handleMessage(int len)
 		  15
 		  The checksum data, always begins with *
 		*/
-		double utc_time = 0.0, lat = 0.0, lon = 0.0;
-		float alt = 0.f, geoid_h = 0.f;
-		float hdop = 99.9f, dgps_age = NAN;
-		int  num_of_sv = 0, fix_quality = 0;
-		char ns = '?', ew = '?';
-
-		NMEA_UNUSED(dgps_age);
-
-		if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { alt = strtof(bufptr, &endp); bufptr = endp; }
-
-		while (*(++bufptr) != ',') {} //skip M
-
-		if (bufptr && *(++bufptr) != ',') { geoid_h = strtof(bufptr, &endp); bufptr = endp; }
-
-		while (*(++bufptr) != ',') {} //skip M
-
-		if (bufptr && *(++bufptr) != ',') { dgps_age = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (ns == 'S') {
-			lat = -lat;
-		}
-
-		if (ew == 'W') {
-			lon = -lon;
-		}
-
-		/* convert from degrees, minutes and seconds to degrees */
-		_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->hdop = hdop;
-		_gps_position->alt = static_cast<int>(alt * 1000);
-		_gps_position->alt_ellipsoid = _gps_position->alt + static_cast<int>(geoid_h * 1000);
-		_sat_num_gga = static_cast<int>(num_of_sv);
-
-
-		if (fix_quality <= 0) {
-			_gps_position->fix_type = 0;
-
-		} else {
-			/*
+        double utc_time = 0.0, lat = 0.0, lon = 0.0;
+        float  alt = 0.f, geoid_h = 0.f;
+        float  hdop = 99.9f, dgps_age = NAN;
+        int    num_of_sv = 0, fix_quality = 0;
+        char   ns = '?', ew = '?';
+
+        NMEA_UNUSED(dgps_age);
+
+        if (bufptr && *(++bufptr) != ',') {
+            utc_time = strtod(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ns = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ew = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            fix_quality = strtol(bufptr, &endp, 10);
+            bufptr      = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            num_of_sv = strtol(bufptr, &endp, 10);
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            hdop   = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            alt    = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        while (*(++bufptr) != ',') {
+        } //skip M
+
+        if (bufptr && *(++bufptr) != ',') {
+            geoid_h = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        while (*(++bufptr) != ',') {
+        } //skip M
+
+        if (bufptr && *(++bufptr) != ',') {
+            dgps_age = strtof(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (ns == 'S') {
+            lat = -lat;
+        }
+
+        if (ew == 'W') {
+            lon = -lon;
+        }
+
+        /* convert from degrees, minutes and seconds to degrees */
+        _gps_position->lon           = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->lat           = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->hdop          = hdop;
+        _gps_position->alt           = static_cast<int>(alt * 1000);
+        _gps_position->alt_ellipsoid = _gps_position->alt + static_cast<int>(geoid_h * 1000);
+        _sat_num_gga                 = static_cast<int>(num_of_sv);
+
+
+        if (fix_quality <= 0) {
+            _gps_position->fix_type = 0;
+
+        } else {
+            /*
 			 * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
 			 * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
 			 */
-			if (fix_quality == 5) { fix_quality = 3; }
+            if (fix_quality == 5) {
+                fix_quality = 3;
+            }
 
-			/*
+            /*
 			 * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
 			 */
-			_gps_position->fix_type = 3 + fix_quality - 1;
-		}
+            _gps_position->fix_type = 3 + fix_quality - 1;
+        }
 
-		if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
-			_last_POS_timeUTC = utc_time;
-			_POS_received = true;
-		}
+        if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
+            _last_POS_timeUTC = utc_time;
+            _POS_received     = true;
+        }
 
-		_ALT_received = true;
-		_SVNUM_received = true;
-		_FIX_received = true;
+        _ALT_received   = true;
+        _SVNUM_received = true;
+        _FIX_received   = true;
 
-		_gps_position->c_variance_rad = 0.1f;
-		_gps_position->timestamp = gps_absolute_time();
+        _gps_position->c_variance_rad = 0.1f;
+        _gps_position->timestamp      = gps_absolute_time();
 
-	} else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && uiCalcComma == 2) {
-		/*
+    } else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && uiCalcComma == 2) {
+        /*
 		Heading message
 		Example $GPHDT,121.2,T*35
 
@@ -311,18 +342,18 @@ int GPSDriverNMEA::handleMessage(int len)
 		T "T" for "True"
 		 */
 
-		float heading_deg = 0.f;
-
-		if (bufptr && *(++bufptr) != ',') {
-			heading_deg = strtof(bufptr, &endp); bufptr = endp;
-			handleHeading(heading_deg, NAN);
-		}
+        float heading_deg = 0.f;
 
-		_HEAD_received = true;
+        if (bufptr && *(++bufptr) != ',') {
+            heading_deg = strtof(bufptr, &endp);
+            bufptr      = endp;
+            handleHeading(heading_deg, NAN);
+        }
 
-	} else if ((memcmp(_rx_buffer + 3, "GNS,", 4) == 0) && (uiCalcComma >= 12)) {
+        _HEAD_received = true;
 
-		/*
+    } else if ((memcmp(_rx_buffer + 3, "GNS,", 4) == 0) && (uiCalcComma >= 12)) {
+        /*
 		Message GNS
 		Type Output Message
 		Time and position, together with GNSS fixing related data (number of satellites in use, and
@@ -351,65 +382,86 @@ int GPSDriverNMEA::handleMessage(int len)
 		14 cs - hexadecimal *71   Checksum
 		15 <CR><LF> - character - Carriage return and line feed
 		*/
-		double utc_time = 0.0;
-		double lat = 0.0, lon = 0.0;
-		char pos_Mode[5] = {'N', 'N', 'N', 'N', 'N'};
-		int num_of_sv = 0;
-		float alt = 0.f;
-		float hdop = 0.f;
-		char ns = '?', ew = '?';
-		int i = 0;
-		NMEA_UNUSED(pos_Mode);
-
-		if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++);}
-
-		do {
-			pos_Mode[i] =  *(bufptr);
-			i++;
-
-		} while (*(++bufptr) != ',' && i < 5);
-
-		if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { alt = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (ns == 'S') {
-			lat = -lat;
-		}
-
-		if (ew == 'W') {
-			lon = -lon;
-		}
-
-		/* convert from degrees, minutes and seconds to degrees */
-		_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->hdop = hdop;
-		_gps_position->alt = static_cast<int>(alt * 1000);
-		_sat_num_gns = static_cast<int>(num_of_sv);
-
-		if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
-			_last_POS_timeUTC = utc_time;
-			_POS_received = true;
-		}
-
-		_ALT_received = true;
-		_SVNUM_received = true;
-
-
-	} else if ((memcmp(_rx_buffer + 3, "RMC,", 4) == 0) && (uiCalcComma >= 11)) {
-
-		/*
+        double utc_time = 0.0;
+        double lat = 0.0, lon = 0.0;
+        char   pos_Mode[5] = {'N', 'N', 'N', 'N', 'N'};
+        int    num_of_sv   = 0;
+        float  alt         = 0.f;
+        float  hdop        = 0.f;
+        char   ns = '?', ew = '?';
+        int    i = 0;
+        NMEA_UNUSED(pos_Mode);
+
+        if (bufptr && *(++bufptr) != ',') {
+            utc_time = strtod(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ns = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ew = *(bufptr++);
+        }
+
+        do {
+            pos_Mode[i] = *(bufptr);
+            i++;
+
+        } while (*(++bufptr) != ',' && i < 5);
+
+        if (bufptr && *(++bufptr) != ',') {
+            num_of_sv = strtol(bufptr, &endp, 10);
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            hdop   = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            alt    = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (ns == 'S') {
+            lat = -lat;
+        }
+
+        if (ew == 'W') {
+            lon = -lon;
+        }
+
+        /* convert from degrees, minutes and seconds to degrees */
+        _gps_position->lat  = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->lon  = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->hdop = hdop;
+        _gps_position->alt  = static_cast<int>(alt * 1000);
+        _sat_num_gns        = static_cast<int>(num_of_sv);
+
+        if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
+            _last_POS_timeUTC = utc_time;
+            _POS_received     = true;
+        }
+
+        _ALT_received   = true;
+        _SVNUM_received = true;
+
+
+    } else if ((memcmp(_rx_buffer + 3, "RMC,", 4) == 0) && (uiCalcComma >= 11)) {
+        /*
 		Position, velocity, and time
 		The RMC string is:
 
@@ -430,133 +482,159 @@ int GPSDriverNMEA::handleMessage(int len)
 		8	Magnetic variation in degrees
 		9	The checksum data, always begins with *
 		*/
-		double utc_time = 0.0;
-		char Status = 'V';
-		double lat = 0.0, lon = 0.0;
-		float ground_speed_K = 0.f;
-		float track_true = 0.f;
-		int nmea_date = 0;
-		float Mag_var = 0.f;
-		char ns = '?', ew = '?';
-		NMEA_UNUSED(Mag_var);
-
-		if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { Status = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { ground_speed_K = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { track_true = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { nmea_date = static_cast<int>(strtol(bufptr, &endp, 10)); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { Mag_var = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (ns == 'S') {
-			lat = -lat;
-		}
-
-		if (ew == 'W') {
-			lon = -lon;
-		}
-
-		if (Status == 'V') {
-			_gps_position->fix_type = 0;
-		}
-
-		float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi]
-
-		if (track_rad > M_PI_F) {
-			track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
-		}
-
-		float velocity_ms = ground_speed_K / 1.9438445f;
-		float velocity_north = velocity_ms * cosf(track_rad);
-		float velocity_east  = velocity_ms * sinf(track_rad);
-		int utc_hour = static_cast<int>(utc_time / 10000);
-		int utc_minute = static_cast<int>((utc_time - utc_hour * 10000) / 100);
-		double utc_sec = static_cast<double>(utc_time - utc_hour * 10000 - utc_minute * 100);
-		int nmea_day = static_cast<int>(nmea_date / 10000);
-		int nmea_mth = static_cast<int>((nmea_date - nmea_day * 10000) / 100);
-		int nmea_year = static_cast<int>(nmea_date - nmea_day * 10000 - nmea_mth * 100);
-		/* convert from degrees, minutes and seconds to degrees */
-		_gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
-		_gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
-
-		_gps_position->vel_m_s = velocity_ms;
-		_gps_position->vel_n_m_s = velocity_north;
-		_gps_position->vel_e_m_s = velocity_east;
-		_gps_position->cog_rad = track_rad;
-		_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
-		_gps_position->c_variance_rad = 0.1f;
-		_gps_position->s_variance_m_s = 0;
-		_gps_position->timestamp = gps_absolute_time();
-		_last_timestamp_time = gps_absolute_time();
-
-		/*
+        double utc_time = 0.0;
+        char   Status   = 'V';
+        double lat = 0.0, lon = 0.0;
+        float  ground_speed_K = 0.f;
+        float  track_true     = 0.f;
+        int    nmea_date      = 0;
+        float  Mag_var        = 0.f;
+        char   ns = '?', ew = '?';
+        NMEA_UNUSED(Mag_var);
+
+        if (bufptr && *(++bufptr) != ',') {
+            utc_time = strtod(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            Status = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ns = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon    = strtod(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ew = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ground_speed_K = strtof(bufptr, &endp);
+            bufptr         = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            track_true = strtof(bufptr, &endp);
+            bufptr     = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            nmea_date = static_cast<int>(strtol(bufptr, &endp, 10));
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            Mag_var = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (ns == 'S') {
+            lat = -lat;
+        }
+
+        if (ew == 'W') {
+            lon = -lon;
+        }
+
+        if (Status == 'V') {
+            _gps_position->fix_type = 0;
+        }
+
+        float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi]
+
+        if (track_rad > M_PI_F) {
+            track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
+        }
+
+        float  velocity_ms    = ground_speed_K / 1.9438445f;
+        float  velocity_north = velocity_ms * cosf(track_rad);
+        float  velocity_east  = velocity_ms * sinf(track_rad);
+        int    utc_hour       = static_cast<int>(utc_time / 10000);
+        int    utc_minute     = static_cast<int>((utc_time - utc_hour * 10000) / 100);
+        double utc_sec        = static_cast<double>(utc_time - utc_hour * 10000 - utc_minute * 100);
+        int    nmea_day       = static_cast<int>(nmea_date / 10000);
+        int    nmea_mth       = static_cast<int>((nmea_date - nmea_day * 10000) / 100);
+        int    nmea_year      = static_cast<int>(nmea_date - nmea_day * 10000 - nmea_mth * 100);
+        /* convert from degrees, minutes and seconds to degrees */
+        _gps_position->lat = static_cast<int>((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
+        _gps_position->lon = static_cast<int>((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
+
+        _gps_position->vel_m_s        = velocity_ms;
+        _gps_position->vel_n_m_s      = velocity_north;
+        _gps_position->vel_e_m_s      = velocity_east;
+        _gps_position->cog_rad        = track_rad;
+        _gps_position->vel_ned_valid  = true; /**< Flag to indicate if NED speed is valid */
+        _gps_position->c_variance_rad = 0.1f;
+        _gps_position->s_variance_m_s = 0;
+        _gps_position->timestamp      = gps_absolute_time();
+        _last_timestamp_time          = gps_absolute_time();
+
+        /*
 		 * convert to unix timestamp
 		 */
-		struct tm timeinfo = {};
-		timeinfo.tm_year = nmea_year + 100;
-		timeinfo.tm_mon = nmea_mth - 1;
-		timeinfo.tm_mday = nmea_day;
-		timeinfo.tm_hour = utc_hour;
-		timeinfo.tm_min = utc_minute;
-		timeinfo.tm_sec = int(utc_sec);
-		timeinfo.tm_isdst = 0;
+        struct tm timeinfo = {};
+        timeinfo.tm_year   = nmea_year + 100;
+        timeinfo.tm_mon    = nmea_mth - 1;
+        timeinfo.tm_mday   = nmea_day;
+        timeinfo.tm_hour   = utc_hour;
+        timeinfo.tm_min    = utc_minute;
+        timeinfo.tm_sec    = int(utc_sec);
+        timeinfo.tm_isdst  = 0;
 
 #ifndef NO_MKTIME
-		time_t epoch = mktime(&timeinfo);
+        time_t epoch = mktime(&timeinfo);
 
-		if (epoch > GPS_EPOCH_SECS) {
-			uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
+        if (epoch > GPS_EPOCH_SECS) {
+            uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
 
-			// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-			// and control its drift. Since we rely on the HRT for our monotonic
-			// clock, updating it from time to time is safe.
-			if (!_clock_set) {
-				timespec ts{};
-				ts.tv_sec = epoch;
-				ts.tv_nsec = usecs * 1000;
+            // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+            // and control its drift. Since we rely on the HRT for our monotonic
+            // clock, updating it from time to time is safe.
+            if (!_clock_set) {
+                timespec ts{};
+                ts.tv_sec  = epoch;
+                ts.tv_nsec = usecs * 1000;
 
-				setClock(ts);
-				_clock_set = true;
-			}
+                setClock(ts);
+                _clock_set = true;
+            }
 
-			_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-			_gps_position->time_utc_usec += usecs;
+            _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+            _gps_position->time_utc_usec += usecs;
 
-		} else {
-			_gps_position->time_utc_usec = 0;
-		}
+        } else {
+            _gps_position->time_utc_usec = 0;
+        }
 
 #else
-		_gps_position->time_utc_usec = 0;
+        _gps_position->time_utc_usec = 0;
 #endif
 
-		if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
-			_last_POS_timeUTC = utc_time;
-			_POS_received = true;
-		}
-
-		if (!_VEL_received && (_last_VEL_timeUTC < utc_time)) {
-			_last_VEL_timeUTC = utc_time;
-			_VEL_received = true;
-		}
+        if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
+            _last_POS_timeUTC = utc_time;
+            _POS_received     = true;
+        }
 
-		_TIME_received = true;
+        if (!_VEL_received && (_last_VEL_timeUTC < utc_time)) {
+            _last_VEL_timeUTC = utc_time;
+            _VEL_received     = true;
+        }
 
-	}	else if ((memcmp(_rx_buffer + 3, "GST,", 4) == 0) && (uiCalcComma == 8)) {
+        _TIME_received = true;
 
-		/*
+    } else if ((memcmp(_rx_buffer + 3, "GST,", 4) == 0) && (uiCalcComma == 8)) {
+        /*
 		Position error statistics
 		An example of the GST message string is:
 
@@ -582,41 +660,64 @@ int GPSDriverNMEA::handleMessage(int len)
 		8   Height 1 sigma error, in meters
 		9   The checksum data, always begins with *
 		*/
-		double utc_time = 0.0;
-		float lat_err = 0.f, lon_err = 0.f, alt_err = 0.f;
-		float min_err = 0.f, maj_err = 0.f, deg_from_north = 0.f, rms_err = 0.f;
-
-		NMEA_UNUSED(min_err);
-		NMEA_UNUSED(maj_err);
-		NMEA_UNUSED(deg_from_north);
-		NMEA_UNUSED(rms_err);
-
-		if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { rms_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { maj_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { min_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { deg_from_north = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lat_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { lon_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { alt_err = strtof(bufptr, &endp); bufptr = endp; }
-
-		_gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
-					   + static_cast<float>(lon_err) * static_cast<float>(lon_err));
-		_gps_position->epv = static_cast<float>(alt_err);
-
-		_EPH_received = true;
-		_last_FIX_timeUTC = utc_time;
-
-	} else if ((memcmp(_rx_buffer + 3, "GSA,", 4) == 0) && (uiCalcComma >= 17)) {
-
-		/*
+        double utc_time = 0.0;
+        float  lat_err = 0.f, lon_err = 0.f, alt_err = 0.f;
+        float  min_err = 0.f, maj_err = 0.f, deg_from_north = 0.f, rms_err = 0.f;
+
+        NMEA_UNUSED(min_err);
+        NMEA_UNUSED(maj_err);
+        NMEA_UNUSED(deg_from_north);
+        NMEA_UNUSED(rms_err);
+
+        if (bufptr && *(++bufptr) != ',') {
+            utc_time = strtod(bufptr, &endp);
+            bufptr   = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            rms_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            maj_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            min_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            deg_from_north = strtof(bufptr, &endp);
+            bufptr         = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lat_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            lon_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            alt_err = strtof(bufptr, &endp);
+            bufptr  = endp;
+        }
+
+        _gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
+                                   + static_cast<float>(lon_err) * static_cast<float>(lon_err));
+        _gps_position->epv = static_cast<float>(alt_err);
+
+        _EPH_received     = true;
+        _last_FIX_timeUTC = utc_time;
+
+    } else if ((memcmp(_rx_buffer + 3, "GSA,", 4) == 0) && (uiCalcComma >= 17)) {
+        /*
 		GPS DOP and active satellites
 		An example of the GSA message string is:
 		$GPGSA,<1>,<2>,<3>,<3>,,,,,<3>,<3>,<3>,<4>,<5>,<6>*<7><CR><LF>
@@ -635,42 +736,58 @@ int GPSDriverNMEA::handleMessage(int len)
 		6	VDOP: 0.5 through 99.9
 		7	The checksum data, always begins with *
 		*/
-		char M_pos = ' ';
-		int fix_mode = 0;
-		int sat_id[12] {0};
-		float pdop = 99.9f, hdop = 99.9f, vdop = 99.9f;
-
-		NMEA_UNUSED(M_pos);
-		NMEA_UNUSED(sat_id);
-		NMEA_UNUSED(pdop);
-
-		if (bufptr && *(++bufptr) != ',') { M_pos = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { fix_mode = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		for (int y = 0; y < 12; y++) {
-			if (bufptr && *(++bufptr) != ',') {sat_id[y] = strtol(bufptr, &endp, 10); bufptr = endp; }
-		}
-
-		if (bufptr && *(++bufptr) != ',') { pdop = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { vdop = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (fix_mode <= 1) {
-			_gps_position->fix_type = 0;
-
-		} else {
-			_gps_position->hdop = static_cast<float>(hdop);
-			_gps_position->vdop = static_cast<float>(vdop);
-			_DOP_received = true;
-
-		}
-
-
-	} else if ((memcmp(_rx_buffer + 3, "GSV,", 4) == 0)) {
-		/*
+        char  M_pos    = ' ';
+        int   fix_mode = 0;
+        int   sat_id[12]{0};
+        float pdop = 99.9f, hdop = 99.9f, vdop = 99.9f;
+
+        NMEA_UNUSED(M_pos);
+        NMEA_UNUSED(sat_id);
+        NMEA_UNUSED(pdop);
+
+        if (bufptr && *(++bufptr) != ',') {
+            M_pos = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            fix_mode = strtol(bufptr, &endp, 10);
+            bufptr   = endp;
+        }
+
+        for (int y = 0; y < 12; y++) {
+            if (bufptr && *(++bufptr) != ',') {
+                sat_id[y] = strtol(bufptr, &endp, 10);
+                bufptr    = endp;
+            }
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            pdop   = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            hdop   = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            vdop   = strtof(bufptr, &endp);
+            bufptr = endp;
+        }
+
+        if (fix_mode <= 1) {
+            _gps_position->fix_type = 0;
+
+        } else {
+            _gps_position->hdop = static_cast<float>(hdop);
+            _gps_position->vdop = static_cast<float>(vdop);
+            _DOP_received       = true;
+        }
+
+
+    } else if ((memcmp(_rx_buffer + 3, "GSV,", 4) == 0)) {
+        /*
 		The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
 
 		$GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
@@ -691,85 +808,105 @@ int GPSDriverNMEA::handleMessage(int len)
 		20  The checksum data, always begins with *
 		*/
 
-		int all_page_num = 0, this_page_num = 0, tot_sv_visible = 0;
-		struct gsv_sat {
-			int svid;
-			int elevation;
-			int azimuth;
-			int snr;
-		} sat[4] {};
-
-		if (bufptr && *(++bufptr) != ',') { all_page_num = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { this_page_num = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-		if ((this_page_num < 1) || (this_page_num > all_page_num)) {
-			return 0;
-		}
-
-		if (memcmp(_rx_buffer, "$GP", 3) == 0) {
-			_sat_num_gpgsv = tot_sv_visible;
-
-		} else if (memcmp(_rx_buffer, "$GL", 3) == 0) {
-			_sat_num_glgsv = tot_sv_visible;
-
-		} else if (memcmp(_rx_buffer, "$GA", 3) == 0) {
-			_sat_num_gagsv = tot_sv_visible;
-
-		} else if (memcmp(_rx_buffer, "$GB", 3) == 0) {
-			_sat_num_gbgsv = tot_sv_visible;
-
-		} else if (memcmp(_rx_buffer, "$BD", 3) == 0) {
-			_sat_num_bdgsv = tot_sv_visible;
-
-		}
-
-		if (this_page_num == 0 && _satellite_info) {
-			memset(_satellite_info->svid,     0, sizeof(_satellite_info->svid));
-			memset(_satellite_info->used,     0, sizeof(_satellite_info->used));
-			memset(_satellite_info->snr,      0, sizeof(_satellite_info->snr));
-			memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
-			memset(_satellite_info->azimuth,  0, sizeof(_satellite_info->azimuth));
-		}
-
-		int end = 4;
-
-		if (this_page_num == all_page_num) {
-			end =  tot_sv_visible - (this_page_num - 1) * 4;
-
-			_SVNUM_received = true;
-			_SVINFO_received = true;
-
-			if (_satellite_info) {
-				_satellite_info->count = satellite_info_s::SAT_INFO_MAX_SATELLITES;
-				_satellite_info->timestamp = gps_absolute_time();
-			}
-		}
-
-		if (_satellite_info) {
-			for (int y = 0 ; y < end ; y++) {
-				if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
-
-				_satellite_info->svid[y + (this_page_num - 1) * 4]      = sat[y].svid;
-				_satellite_info->used[y + (this_page_num - 1) * 4]      = (sat[y].snr > 0);
-				_satellite_info->snr[y + (this_page_num - 1) * 4]       = sat[y].snr;
-				_satellite_info->elevation[y + (this_page_num - 1) * 4] = sat[y].elevation;
-				_satellite_info->azimuth[y + (this_page_num - 1) * 4]   = sat[y].azimuth;
-			}
-		}
-
-
-	} else if ((memcmp(_rx_buffer + 3, "VTG,", 4) == 0) && (uiCalcComma >= 8)) {
-
-		/*$GNVTG,,T,,M,0.683,N,1.265,K*30
+        int all_page_num = 0, this_page_num = 0, tot_sv_visible = 0;
+
+        struct gsv_sat {
+            int svid;
+            int elevation;
+            int azimuth;
+            int snr;
+        } sat[4]{};
+
+        if (bufptr && *(++bufptr) != ',') {
+            all_page_num = strtol(bufptr, &endp, 10);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            this_page_num = strtol(bufptr, &endp, 10);
+            bufptr        = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            tot_sv_visible = strtol(bufptr, &endp, 10);
+            bufptr         = endp;
+        }
+
+        if ((this_page_num < 1) || (this_page_num > all_page_num)) {
+            return 0;
+        }
+
+        if (memcmp(_rx_buffer, "$GP", 3) == 0) {
+            _sat_num_gpgsv = tot_sv_visible;
+
+        } else if (memcmp(_rx_buffer, "$GL", 3) == 0) {
+            _sat_num_glgsv = tot_sv_visible;
+
+        } else if (memcmp(_rx_buffer, "$GA", 3) == 0) {
+            _sat_num_gagsv = tot_sv_visible;
+
+        } else if (memcmp(_rx_buffer, "$GB", 3) == 0) {
+            _sat_num_gbgsv = tot_sv_visible;
+
+        } else if (memcmp(_rx_buffer, "$BD", 3) == 0) {
+            _sat_num_bdgsv = tot_sv_visible;
+        }
+
+        if (this_page_num == 0 && _satellite_info) {
+            memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
+            memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
+            memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
+            memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
+            memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
+        }
+
+        int end = 4;
+
+        if (this_page_num == all_page_num) {
+            end = tot_sv_visible - (this_page_num - 1) * 4;
+
+            _SVNUM_received  = true;
+            _SVINFO_received = true;
+
+            if (_satellite_info) {
+                _satellite_info->count     = satellite_info_s::SAT_INFO_MAX_SATELLITES;
+                _satellite_info->timestamp = gps_absolute_time();
+            }
+        }
+
+        if (_satellite_info) {
+            for (int y = 0; y < end; y++) {
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].svid = strtol(bufptr, &endp, 10);
+                    bufptr      = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].elevation = strtol(bufptr, &endp, 10);
+                    bufptr           = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].azimuth = strtol(bufptr, &endp, 10);
+                    bufptr         = endp;
+                }
+
+                if (bufptr && *(++bufptr) != ',') {
+                    sat[y].snr = strtol(bufptr, &endp, 10);
+                    bufptr     = endp;
+                }
+
+                _satellite_info->svid[y + (this_page_num - 1) * 4]      = sat[y].svid;
+                _satellite_info->used[y + (this_page_num - 1) * 4]      = (sat[y].snr > 0);
+                _satellite_info->snr[y + (this_page_num - 1) * 4]       = sat[y].snr;
+                _satellite_info->elevation[y + (this_page_num - 1) * 4] = sat[y].elevation;
+                _satellite_info->azimuth[y + (this_page_num - 1) * 4]   = sat[y].azimuth;
+            }
+        }
+
+
+    } else if ((memcmp(_rx_buffer + 3, "VTG,", 4) == 0) && (uiCalcComma >= 8)) {
+        /*$GNVTG,,T,,M,0.683,N,1.265,K*30
 		  $GNVTG,,T,,M,0.780,N,1.445,K*33
 
 		Field	Meaning
@@ -785,326 +922,337 @@ int GPSDriverNMEA::handleMessage(int len)
 		9	The checksum data, always begins with *
 		*/
 
-		float track_true = 0.f;
-		char T;
-		float track_mag = 0.f;
-		char M;
-		float ground_speed = 0.f;
-		char N;
-		float ground_speed_K = 0.f;
-		char K;
-		NMEA_UNUSED(T);
-		NMEA_UNUSED(track_mag);
-		NMEA_UNUSED(M);
-		NMEA_UNUSED(N);
-		NMEA_UNUSED(ground_speed_K);
-		NMEA_UNUSED(K);
-
-		if (bufptr && *(++bufptr) != ',') {track_true = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { T = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') {track_mag = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { M = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { ground_speed = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { N = *(bufptr++); }
-
-		if (bufptr && *(++bufptr) != ',') { ground_speed_K = strtof(bufptr, &endp); bufptr = endp; }
-
-		if (bufptr && *(++bufptr) != ',') { K = *(bufptr++); }
-
-		float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi]
-
-		if (track_rad > M_PI_F) {
-			track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
-		}
-
-		float velocity_ms = ground_speed / 1.9438445f;
-		float velocity_north = velocity_ms * cosf(track_rad);
-		float velocity_east  = velocity_ms * sinf(track_rad);
-
-		_gps_position->vel_m_s = velocity_ms;
-		_gps_position->vel_n_m_s = velocity_north;
-		_gps_position->vel_e_m_s = velocity_east;
-		_gps_position->cog_rad = track_rad;
-		_gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */
-		_gps_position->c_variance_rad = 0.1f;
-		_gps_position->s_variance_m_s = 0;
-
-		if (!_VEL_received) {
-			_VEL_received = true;
-		}
-	}
-
-	if (_sat_num_gga > 0) {
-		_gps_position->satellites_used = _sat_num_gga;
-
-	} else if (_SVNUM_received && _SVINFO_received && _FIX_received) {
-
-		_sat_num_gsv = _sat_num_gpgsv + _sat_num_glgsv + _sat_num_gagsv
-			       + _sat_num_gbgsv + _sat_num_bdgsv;
-		_gps_position->satellites_used = MAX(_sat_num_gns, _sat_num_gsv);
-	}
-
-	if (_VEL_received && _POS_received) {
-		ret = 1;
-		_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
-		_clock_set = false;
-		_VEL_received = false;
-		_POS_received = false;
-		_rate_count_vel++;
-		_rate_count_lat_lon++;
-	}
-
-	return ret;
+        float track_true = 0.f;
+        char  T;
+        float track_mag = 0.f;
+        char  M;
+        float ground_speed = 0.f;
+        char  N;
+        float ground_speed_K = 0.f;
+        char  K;
+        NMEA_UNUSED(T);
+        NMEA_UNUSED(track_mag);
+        NMEA_UNUSED(M);
+        NMEA_UNUSED(N);
+        NMEA_UNUSED(ground_speed_K);
+        NMEA_UNUSED(K);
+
+        if (bufptr && *(++bufptr) != ',') {
+            track_true = strtof(bufptr, &endp);
+            bufptr     = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            T = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            track_mag = strtof(bufptr, &endp);
+            bufptr    = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            M = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ground_speed = strtof(bufptr, &endp);
+            bufptr       = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            N = *(bufptr++);
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            ground_speed_K = strtof(bufptr, &endp);
+            bufptr         = endp;
+        }
+
+        if (bufptr && *(++bufptr) != ',') {
+            K = *(bufptr++);
+        }
+
+        float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi]
+
+        if (track_rad > M_PI_F) {
+            track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
+        }
+
+        float velocity_ms    = ground_speed / 1.9438445f;
+        float velocity_north = velocity_ms * cosf(track_rad);
+        float velocity_east  = velocity_ms * sinf(track_rad);
+
+        _gps_position->vel_m_s        = velocity_ms;
+        _gps_position->vel_n_m_s      = velocity_north;
+        _gps_position->vel_e_m_s      = velocity_east;
+        _gps_position->cog_rad        = track_rad;
+        _gps_position->vel_ned_valid  = true; /** Flag to indicate if NED speed is valid */
+        _gps_position->c_variance_rad = 0.1f;
+        _gps_position->s_variance_m_s = 0;
+
+        if (!_VEL_received) {
+            _VEL_received = true;
+        }
+    }
+
+    if (_sat_num_gga > 0) {
+        _gps_position->satellites_used = _sat_num_gga;
+
+    } else if (_SVNUM_received && _SVINFO_received && _FIX_received) {
+        _sat_num_gsv = _sat_num_gpgsv + _sat_num_glgsv + _sat_num_gagsv
+                     + _sat_num_gbgsv + _sat_num_bdgsv;
+        _gps_position->satellites_used = MAX(_sat_num_gns, _sat_num_gsv);
+    }
+
+    if (_VEL_received && _POS_received) {
+        ret                                    = 1;
+        _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
+        _clock_set                             = false;
+        _VEL_received                          = false;
+        _POS_received                          = false;
+        _rate_count_vel++;
+        _rate_count_lat_lon++;
+    }
+
+    return ret;
 }
 
-int GPSDriverNMEA::receive(unsigned timeout)
-{
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
-
-	/* timeout additional to poll */
-	gps_abstime time_started = gps_absolute_time();
-
-	int handled = 0;
-
-	while (true) {
-		int ret = read(buf, sizeof(buf), timeout);
-
-		if (ret < 0) {
-			/* something went wrong when polling or reading */
-			NMEA_WARN("poll_or_read err");
-			return -1;
-
-		} else if (ret != 0) {
-
-			/* pass received bytes to the packet decoder */
-			for (int i = 0; i < ret; i++) {
-				int l = parseChar(buf[i]);
-
-				if (l > 0) {
-					handled |= handleMessage(l);
-				}
-
-				UnicoreParser::Result result = _unicore_parser.parseChar(buf[i]);
-
-				if (result == UnicoreParser::Result::GotHeading) {
-					++handled;
-					_unicore_heading_received_last = gps_absolute_time();
-
-					// Unicore seems to publish heading and standard deviation of 0
-					// to signal that it has not initialized the heading yet.
-					if (_unicore_parser.heading().heading_stddev_deg > 0.0f) {
-						// Unicore publishes the heading between True North and
-						// the baseline vector from master antenna to slave
-						// antenna.
-						// Assuming that the master is in front and the slave
-						// in the back, this means that we need to flip the
-						// heading 180 degrees.
-
-						handleHeading(
-							_unicore_parser.heading().heading_deg + 180.0f,
-							_unicore_parser.heading().heading_stddev_deg);
-					}
-
-					NMEA_DEBUG("Got heading: %.1f deg, stddev: %.1f deg, baseline: %.2f m\n",
-						   (double)_unicore_parser.heading().heading_deg,
-						   (double)_unicore_parser.heading().heading_stddev_deg,
-						   (double)_unicore_parser.heading().baseline_m);
-
-				} else if (result == UnicoreParser::Result::GotAgrica) {
-					++handled;
-
-					// We don't use anything of that message at this point, however, this
-					// allows to determine whether we are talking to a UM982 and hence
-					// request the heading (UNIHEADINGA) message that we actually require.
-
-					if (gps_absolute_time() - _unicore_heading_received_last > 1000000) {
-						request_unicore_heading_message();
-					}
-				}
-			}
-
-			if (handled > 0) {
-				return handled;
-			}
-		}
-
-		/* abort after timeout if no useful packets received */
-		if (time_started + timeout * 1000 < gps_absolute_time()) {
-			return -1;
-		}
-	}
+int GPSDriverNMEA::receive(unsigned timeout) {
+    uint8_t buf[GPS_READ_BUFFER_SIZE];
+
+    /* timeout additional to poll */
+    gps_abstime time_started = gps_absolute_time();
+
+    int handled = 0;
+
+    while (true) {
+        int ret = read(buf, sizeof(buf), timeout);
+
+        if (ret < 0) {
+            /* something went wrong when polling or reading */
+            NMEA_WARN("poll_or_read err");
+            return -1;
+
+        } else if (ret != 0) {
+            /* pass received bytes to the packet decoder */
+            for (int i = 0; i < ret; i++) {
+                int l = parseChar(buf[i]);
+
+                if (l > 0) {
+                    handled |= handleMessage(l);
+                }
+
+                UnicoreParser::Result result = _unicore_parser.parseChar(buf[i]);
+
+                if (result == UnicoreParser::Result::GotHeading) {
+                    ++handled;
+                    _unicore_heading_received_last = gps_absolute_time();
+
+                    // Unicore seems to publish heading and standard deviation of 0
+                    // to signal that it has not initialized the heading yet.
+                    if (_unicore_parser.heading().heading_stddev_deg > 0.0f) {
+                        // Unicore publishes the heading between True North and
+                        // the baseline vector from master antenna to slave
+                        // antenna.
+                        // Assuming that the master is in front and the slave
+                        // in the back, this means that we need to flip the
+                        // heading 180 degrees.
+
+                        handleHeading(
+                            _unicore_parser.heading().heading_deg + 180.0f,
+                            _unicore_parser.heading().heading_stddev_deg);
+                    }
+
+                    NMEA_DEBUG("Got heading: %.1f deg, stddev: %.1f deg, baseline: %.2f m\n",
+                               (double)_unicore_parser.heading().heading_deg,
+                               (double)_unicore_parser.heading().heading_stddev_deg,
+                               (double)_unicore_parser.heading().baseline_m);
+
+                } else if (result == UnicoreParser::Result::GotAgrica) {
+                    ++handled;
+
+                    // We don't use anything of that message at this point, however, this
+                    // allows to determine whether we are talking to a UM982 and hence
+                    // request the heading (UNIHEADINGA) message that we actually require.
+
+                    if (gps_absolute_time() - _unicore_heading_received_last > 1000000) {
+                        request_unicore_heading_message();
+                    }
+                }
+            }
+
+            if (handled > 0) {
+                return handled;
+            }
+        }
+
+        /* abort after timeout if no useful packets received */
+        if (time_started + timeout * 1000 < gps_absolute_time()) {
+            return -1;
+        }
+    }
 }
 
-void GPSDriverNMEA::handleHeading(float heading_deg, float heading_stddev_deg)
-{
-	float heading_rad = heading_deg * M_PI_F / 180.0f; // rad in range [0, 2pi]
-	heading_rad -= _heading_offset; // rad in range [-pi, 3pi]
+void GPSDriverNMEA::handleHeading(float heading_deg, float heading_stddev_deg) {
+    float heading_rad  = heading_deg * M_PI_F / 180.0f; // rad in range [0, 2pi]
+    heading_rad       -= _heading_offset;               // rad in range [-pi, 3pi]
 
-	if (heading_rad > M_PI_F) {
-		heading_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
-	}
+    if (heading_rad > M_PI_F) {
+        heading_rad -= 2.f * M_PI_F; // rad in range [-pi, pi]
+    }
 
-	// We are not publishing heading_offset because it wasn't done in the past,
-	// and the UBX driver doesn't do it either. I'm assuming it would cause the
-	// offset to be applied twice.
+    // We are not publishing heading_offset because it wasn't done in the past,
+    // and the UBX driver doesn't do it either. I'm assuming it would cause the
+    // offset to be applied twice.
 
-	_gps_position->heading = heading_rad;
+    _gps_position->heading = heading_rad;
 
-	const float heading_stddev_rad = heading_stddev_deg * M_PI_F / 180.0f;
-	_gps_position->heading_accuracy = heading_stddev_rad;
+    const float heading_stddev_rad  = heading_stddev_deg * M_PI_F / 180.0f;
+    _gps_position->heading_accuracy = heading_stddev_rad;
 }
 
-void GPSDriverNMEA::request_unicore_heading_message()
-{
-	// Configure heading message on serial port at 5 Hz. Don't save it though.
-	uint8_t buf[] = "UNIHEADINGA COM1 0.2\r\n";
-	write(buf, sizeof(buf) - 1);
+void GPSDriverNMEA::request_unicore_heading_message() {
+    // Configure heading message on serial port at 5 Hz. Don't save it though.
+    uint8_t buf[] = "UNIHEADINGA COM1 0.2\r\n";
+    write(buf, sizeof(buf) - 1);
 }
 
-#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
-
-int GPSDriverNMEA::parseChar(uint8_t b)
-{
-	int iRet = 0;
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A' - 0xA)))
 
-	switch (_decode_state) {
-	/* First, look for sync1 */
-	case NMEADecodeState::uninit:
-		if (b == '$') {
-			_decode_state = NMEADecodeState::got_sync1;
-			_rx_buffer_bytes = 0;
-			_rx_buffer[_rx_buffer_bytes++] = b;
+int GPSDriverNMEA::parseChar(uint8_t b) {
+    int iRet = 0;
 
-		}  else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
-			_decode_state = NMEADecodeState::decode_rtcm3;
-			_rtcm_parsing->addByte(b);
+    switch (_decode_state) {
+    /* First, look for sync1 */
+    case NMEADecodeState::uninit:
+        if (b == '$') {
+            _decode_state                  = NMEADecodeState::got_sync1;
+            _rx_buffer_bytes               = 0;
+            _rx_buffer[_rx_buffer_bytes++] = b;
 
-		}
+        } else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
+            _decode_state = NMEADecodeState::decode_rtcm3;
+            _rtcm_parsing->addByte(b);
+        }
 
-		break;
+        break;
 
-	case NMEADecodeState::got_sync1:
-		if (b == '$') {
-			_decode_state = NMEADecodeState::got_sync1;
-			_rx_buffer_bytes = 0;
+    case NMEADecodeState::got_sync1:
+        if (b == '$') {
+            _decode_state    = NMEADecodeState::got_sync1;
+            _rx_buffer_bytes = 0;
 
-		} else if (b == '*') {
-			_decode_state = NMEADecodeState::got_asteriks;
-		}
+        } else if (b == '*') {
+            _decode_state = NMEADecodeState::got_asteriks;
+        }
 
-		if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
-			_decode_state = NMEADecodeState::uninit;
-			_rx_buffer_bytes = 0;
+        if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
+            _decode_state    = NMEADecodeState::uninit;
+            _rx_buffer_bytes = 0;
 
-		} else {
-			_rx_buffer[_rx_buffer_bytes++] = b;
-		}
+        } else {
+            _rx_buffer[_rx_buffer_bytes++] = b;
+        }
 
-		break;
+        break;
 
-	case NMEADecodeState::got_asteriks:
-		_rx_buffer[_rx_buffer_bytes++] = b;
-		_decode_state = NMEADecodeState::got_first_cs_byte;
-		break;
+    case NMEADecodeState::got_asteriks:
+        _rx_buffer[_rx_buffer_bytes++] = b;
+        _decode_state                  = NMEADecodeState::got_first_cs_byte;
+        break;
 
-	case NMEADecodeState::got_first_cs_byte: {
-			_rx_buffer[_rx_buffer_bytes++] = b;
-			uint8_t checksum = 0;
-			uint8_t *buffer = _rx_buffer + 1;
-			uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
+    case NMEADecodeState::got_first_cs_byte: {
+        _rx_buffer[_rx_buffer_bytes++] = b;
+        uint8_t  checksum              = 0;
+        uint8_t *buffer                = _rx_buffer + 1;
+        uint8_t *bufend                = _rx_buffer + _rx_buffer_bytes - 3;
 
-			for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
+        for (; buffer < bufend; buffer++) {
+            checksum ^= *buffer;
+        }
 
-			if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
-			    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
-				iRet = _rx_buffer_bytes;
-			}
+        if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
+            iRet = _rx_buffer_bytes;
+        }
 
-			decodeInit();
-		}
-		break;
+        decodeInit();
+    } break;
 
-	case NMEADecodeState::decode_rtcm3:
-		if (_rtcm_parsing->addByte(b)) {
-			NMEA_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
-			gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
-			decodeInit();
-		}
+    case NMEADecodeState::decode_rtcm3:
+        if (_rtcm_parsing->addByte(b)) {
+            NMEA_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
+            gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
+            decodeInit();
+        }
 
-		break;
-	}
+        break;
+    }
 
-	return iRet;
+    return iRet;
 }
 
-void GPSDriverNMEA::decodeInit()
-{
-	_rx_buffer_bytes = 0;
-	_decode_state = NMEADecodeState::uninit;
+void GPSDriverNMEA::decodeInit() {
+    _rx_buffer_bytes = 0;
+    _decode_state    = NMEADecodeState::uninit;
 
-	if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
-		if (!_rtcm_parsing) {
-			_rtcm_parsing = new RTCMParsing();
-		}
+    if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
+        if (!_rtcm_parsing) {
+            _rtcm_parsing = new RTCMParsing();
+        }
 
-		if (_rtcm_parsing) {
-			_rtcm_parsing->reset();
-		}
-	}
+        if (_rtcm_parsing) {
+            _rtcm_parsing->reset();
+        }
+    }
 }
 
-int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	_output_mode = config.output_mode;
-
-	if (_output_mode != OutputMode::GPS) {
-		NMEA_WARN("RTCM output have to be configured manually");
-	}
-
-	// If a baudrate is defined, we test this first
-	if (baudrate > 0) {
-		setBaudrate(baudrate);
-		decodeInit();
-		int ret = receive(400);
-		gps_usleep(2000);
-
-		// If a valid POS message is received we have GPS
-		if (_POS_received || ret > 0) {
-			return 0;
-		}
-	}
-
-	// If we haven't found the GPS with the defined baudrate, we try other rates
-	const unsigned baudrates_to_try[] = {9600, 19200, 38400, 57600, 115200, 230400};
-	unsigned test_baudrate;
-
-	for (unsigned int baud_i = 0; !_POS_received
-	     && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
-
-		test_baudrate = baudrates_to_try[baud_i];
-		setBaudrate(test_baudrate);
-
-		NMEA_DEBUG("baudrate set to %i", test_baudrate);
-
-		decodeInit();
-		int ret = receive(400);
-		gps_usleep(2000);
-
-		// If a valid POS message is received we have GPS
-		if (_POS_received || ret > 0) {
-			return 0;
-		}
-	}
-
-	// If nothing is found we leave the specified or default
-	if (baudrate > 0) {
-		return setBaudrate(baudrate);
-	}
-
-	return setBaudrate(NMEA_DEFAULT_BAUDRATE);
+int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config) {
+    _output_mode = config.output_mode;
+
+    if (_output_mode != OutputMode::GPS) {
+        NMEA_WARN("RTCM output have to be configured manually");
+    }
+
+    // If a baudrate is defined, we test this first
+    if (baudrate > 0) {
+        setBaudrate(baudrate);
+        decodeInit();
+        int ret = receive(400);
+        gps_usleep(2000);
+
+        // If a valid POS message is received we have GPS
+        if (_POS_received || ret > 0) {
+            return 0;
+        }
+    }
+
+    // If we haven't found the GPS with the defined baudrate, we try other rates
+    const unsigned baudrates_to_try[] = {9600, 19200, 38400, 57600, 115200, 230400};
+    unsigned       test_baudrate;
+
+    for (unsigned int baud_i = 0; !_POS_received
+                                  && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]);
+         baud_i++) {
+        test_baudrate = baudrates_to_try[baud_i];
+        setBaudrate(test_baudrate);
+
+        NMEA_DEBUG("baudrate set to %i", test_baudrate);
+
+        decodeInit();
+        int ret = receive(400);
+        gps_usleep(2000);
+
+        // If a valid POS message is received we have GPS
+        if (_POS_received || ret > 0) {
+            return 0;
+        }
+    }
+
+    // If nothing is found we leave the specified or default
+    if (baudrate > 0) {
+        return setBaudrate(baudrate);
+    }
+
+    return setBaudrate(NMEA_DEFAULT_BAUDRATE);
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/nmea.h b/apps/peripheral/sensor/gnss/gps/devices/src/nmea.h
index b02432945d..deaeacd039 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/nmea.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/nmea.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020, 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file nmea.h
@@ -53,82 +30,81 @@ class RTCMParsing;
 #define NMEA_RECV_BUFFER_SIZE 1024
 #define NMEA_DEFAULT_BAUDRATE 115200
 
-class GPSDriverNMEA : public GPSHelper
-{
+class GPSDriverNMEA : public GPSHelper {
 public:
-	/**
+    /**
 	 * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement.
 	 */
-	GPSDriverNMEA(GPSCallbackPtr callback, void *callback_user,
-		      sensor_gps_s *gps_position,
-		      satellite_info_s *satellite_info,
-		      float heading_offset = 0.f);
+    GPSDriverNMEA(GPSCallbackPtr callback, void *callback_user,
+                  sensor_gps_s     *gps_position,
+                  satellite_info_s *satellite_info,
+                  float             heading_offset = 0.f);
 
-	virtual ~GPSDriverNMEA();
+    virtual ~GPSDriverNMEA();
 
-	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int receive(unsigned timeout) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
-	void handleHeading(float heading_deg, float heading_stddev_deg);
-	void request_unicore_heading_message();
-
-	UnicoreParser _unicore_parser;
-	gps_abstime _unicore_heading_received_last;
-
-	enum class NMEADecodeState {
-		uninit,
-		got_sync1,
-		got_asteriks,
-		got_first_cs_byte,
-		decode_rtcm3
-	};
-
-	void decodeInit(void);
-	int handleMessage(int len);
-	int parseChar(uint8_t b);
-
-	int32_t read_int();
-	double read_float();
-	char read_char();
-
-	sensor_gps_s *_gps_position {nullptr};
-	satellite_info_s *_satellite_info {nullptr};
-	double _last_POS_timeUTC{0};
-	double _last_VEL_timeUTC{0};
-	double _last_FIX_timeUTC{0};
-	uint64_t _last_timestamp_time{0};
-
-	uint8_t _sat_num_gga{0};
-	uint8_t _sat_num_gns{0};
-	uint8_t _sat_num_gsv{0};
-	uint8_t _sat_num_gpgsv{0};
-	uint8_t _sat_num_glgsv{0};
-	uint8_t _sat_num_gagsv{0};
-	uint8_t _sat_num_gbgsv{0};
-	uint8_t _sat_num_bdgsv{0};
-
-	bool _clock_set {false};
-
-//  check if we got all basic essential packages we need
-	bool _TIME_received{false};
-	bool _POS_received{false};
-	bool _ALT_received{false};
-	bool _SVNUM_received{false};
-	bool _SVINFO_received{false};
-	bool _FIX_received{false};
-	bool _DOP_received{false};
-	bool _VEL_received{false};
-	bool _EPH_received{false};
-	bool _HEAD_received{false};
-
-	NMEADecodeState _decode_state{NMEADecodeState::uninit};
-	uint8_t _rx_buffer[NMEA_RECV_BUFFER_SIZE] {};
-	uint16_t _rx_buffer_bytes{0};
-
-	OutputMode _output_mode{OutputMode::GPS};
-
-	RTCMParsing *_rtcm_parsing{nullptr};
-
-	float _heading_offset;
+    void handleHeading(float heading_deg, float heading_stddev_deg);
+    void request_unicore_heading_message();
+
+    UnicoreParser _unicore_parser;
+    gps_abstime   _unicore_heading_received_last;
+
+    enum class NMEADecodeState {
+        uninit,
+        got_sync1,
+        got_asteriks,
+        got_first_cs_byte,
+        decode_rtcm3
+    };
+
+    void decodeInit(void);
+    int  handleMessage(int len);
+    int  parseChar(uint8_t b);
+
+    int32_t read_int();
+    double  read_float();
+    char    read_char();
+
+    sensor_gps_s     *_gps_position{nullptr};
+    satellite_info_s *_satellite_info{nullptr};
+    double            _last_POS_timeUTC{0};
+    double            _last_VEL_timeUTC{0};
+    double            _last_FIX_timeUTC{0};
+    uint64_t          _last_timestamp_time{0};
+
+    uint8_t _sat_num_gga{0};
+    uint8_t _sat_num_gns{0};
+    uint8_t _sat_num_gsv{0};
+    uint8_t _sat_num_gpgsv{0};
+    uint8_t _sat_num_glgsv{0};
+    uint8_t _sat_num_gagsv{0};
+    uint8_t _sat_num_gbgsv{0};
+    uint8_t _sat_num_bdgsv{0};
+
+    bool _clock_set{false};
+
+    //  check if we got all basic essential packages we need
+    bool _TIME_received{false};
+    bool _POS_received{false};
+    bool _ALT_received{false};
+    bool _SVNUM_received{false};
+    bool _SVINFO_received{false};
+    bool _FIX_received{false};
+    bool _DOP_received{false};
+    bool _VEL_received{false};
+    bool _EPH_received{false};
+    bool _HEAD_received{false};
+
+    NMEADecodeState _decode_state{NMEADecodeState::uninit};
+    uint8_t         _rx_buffer[NMEA_RECV_BUFFER_SIZE]{};
+    uint16_t        _rx_buffer_bytes{0};
+
+    OutputMode _output_mode{OutputMode::GPS};
+
+    RTCMParsing *_rtcm_parsing{nullptr};
+
+    float _heading_offset;
 };
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.cpp
index d98dc2932a..3a55b9ebac 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.cpp
@@ -1,87 +1,60 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "rtcm.h"
 #include <cstring>
 
-RTCMParsing::RTCMParsing()
-{
-	reset();
+RTCMParsing::RTCMParsing() {
+    reset();
 }
 
-RTCMParsing::~RTCMParsing()
-{
-	delete[] _buffer;
+RTCMParsing::~RTCMParsing() {
+    delete[] _buffer;
 }
 
-void RTCMParsing::reset()
-{
-	if (!_buffer) {
-		_buffer = new uint8_t[RTCM_INITIAL_BUFFER_LENGTH];
-		_buffer_len = RTCM_INITIAL_BUFFER_LENGTH;
-	}
+void RTCMParsing::reset() {
+    if (!_buffer) {
+        _buffer     = new uint8_t[RTCM_INITIAL_BUFFER_LENGTH];
+        _buffer_len = RTCM_INITIAL_BUFFER_LENGTH;
+    }
 
-	_pos = 0;
-	_message_length = _buffer_len;
+    _pos            = 0;
+    _message_length = _buffer_len;
 }
 
-bool RTCMParsing::addByte(uint8_t b)
-{
-	if (!_buffer) {
-		return false;
-	}
+bool RTCMParsing::addByte(uint8_t b) {
+    if (!_buffer) {
+        return false;
+    }
 
-	_buffer[_pos++] = b;
+    _buffer[_pos++] = b;
 
-	if (_pos == 3) {
-		_message_length = (((uint16_t)_buffer[1] & 3) << 8) | (_buffer[2]);
+    if (_pos == 3) {
+        _message_length = (((uint16_t)_buffer[1] & 3) << 8) | (_buffer[2]);
 
-		if (_message_length + 6 > _buffer_len) {
-			uint16_t new_buffer_len = _message_length + 6;
-			uint8_t *new_buffer = new uint8_t[new_buffer_len];
+        if (_message_length + 6 > _buffer_len) {
+            uint16_t new_buffer_len = _message_length + 6;
+            uint8_t *new_buffer     = new uint8_t[new_buffer_len];
 
-			if (!new_buffer) {
-				delete[](_buffer);
-				_buffer = nullptr;
-				return false;
-			}
+            if (!new_buffer) {
+                delete[] (_buffer);
+                _buffer = nullptr;
+                return false;
+            }
 
-			memcpy(new_buffer, _buffer, 3);
-			delete[](_buffer);
-			_buffer = new_buffer;
-			_buffer_len = new_buffer_len;
-		}
-	}
+            memcpy(new_buffer, _buffer, 3);
+            delete[] (_buffer);
+            _buffer     = new_buffer;
+            _buffer_len = new_buffer_len;
+        }
+    }
 
-	return _message_length + 6 == _pos;
+    return _message_length + 6 == _pos;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.h b/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.h
index 744e4a07a0..a4e06a29fa 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/rtcm.h
@@ -1,70 +1,53 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
 #include <cstdint>
 
 /* RTCM3 */
-#define RTCM3_PREAMBLE					0xD3
-#define RTCM_INITIAL_BUFFER_LENGTH			300		/**< initial maximum message length of an RTCM message */
+#define RTCM3_PREAMBLE             0xD3
+#define RTCM_INITIAL_BUFFER_LENGTH 300 /**< initial maximum message length of an RTCM message */
 
-
-class RTCMParsing
-{
+class RTCMParsing {
 public:
-	RTCMParsing();
-	~RTCMParsing();
+    RTCMParsing();
+    ~RTCMParsing();
 
-	/**
+    /**
 	 * reset the parsing state
 	 */
-	void reset();
+    void reset();
 
-	/**
+    /**
 	 * add a byte to the message
 	 * @param b
 	 * @return true if message complete (use @message to get it)
 	 */
-	bool addByte(uint8_t b);
+    bool addByte(uint8_t b);
+
+    uint8_t *message() const {
+        return _buffer;
+    }
+
+    uint16_t messageLength() const {
+        return _pos;
+    }
 
-	uint8_t *message() const { return _buffer; }
-	uint16_t messageLength() const { return _pos; }
-	uint16_t messageId() const { return (_buffer[3] << 4) | (_buffer[4] >> 4); }
+    uint16_t messageId() const {
+        return (_buffer[3] << 4) | (_buffer[4] >> 4);
+    }
 
 private:
-	uint8_t			*_buffer{nullptr};
-	uint16_t		_buffer_len{};
-	uint16_t		_pos{};						///< next position in buffer
-	uint16_t		_message_length{};					///< message length without header & CRC (both 3 bytes)
+    uint8_t *_buffer{nullptr};
+    uint16_t _buffer_len{};
+    uint16_t _pos{};            ///< next position in buffer
+    uint16_t _message_length{}; ///< message length without header & CRC (both 3 bytes)
 };
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/sbf.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/sbf.cpp
index b9dd1e4f01..2bc7a82c8d 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/sbf.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/sbf.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file sbf.cpp
@@ -48,676 +25,665 @@
 #include <ctime>
 #include <cmath>
 
-#define SBF_CONFIG_TIMEOUT        1000      // ms, timeout for waiting ACK
-#define SBF_PACKET_TIMEOUT        2        // ms, if now data during this delay assume that full update received
-#define DISABLE_MSG_INTERVAL    1000000  // us, try to disable message with this interval
-#define DNU                        100000.0 // Do-Not-Use value for PVTGeodetic
-#define MSG_SIZE                    100 // size of the message to be sent to the receiver.
+#define SBF_CONFIG_TIMEOUT   1000     // ms, timeout for waiting ACK
+#define SBF_PACKET_TIMEOUT   2        // ms, if now data during this delay assume that full update received
+#define DISABLE_MSG_INTERVAL 1000000  // us, try to disable message with this interval
+#define DNU                  100000.0 // Do-Not-Use value for PVTGeodetic
+#define MSG_SIZE             100      // size of the message to be sent to the receiver.
 
 /**** Trace macros, disable for production builds */
-#define SBF_TRACE_PARSER(...)   {/*GPS_INFO(__VA_ARGS__);*/}    /* decoding progress in parse_char() */
-#define SBF_TRACE_RXMSG(...)    {/*GPS_INFO(__VA_ARGS__);*/}    /* Rx msgs in payload_rx_done() */
-#define SBF_INFO(...)           {GPS_INFO(__VA_ARGS__);}
+#define SBF_TRACE_PARSER(...)    \
+    { /*GPS_INFO(__VA_ARGS__);*/ \
+    } /* decoding progress in parse_char() */
+#define SBF_TRACE_RXMSG(...)     \
+    { /*GPS_INFO(__VA_ARGS__);*/ \
+    } /* Rx msgs in payload_rx_done() */
+#define SBF_INFO(...) \
+    { GPS_INFO(__VA_ARGS__); }
 
 /**** Warning macros, disable to save memory */
-#define SBF_WARN(...)        {GPS_WARN(__VA_ARGS__);}
-#define SBF_DEBUG(...)       {/*GPS_WARN(__VA_ARGS__);*/}
+#define SBF_WARN(...) \
+    { GPS_WARN(__VA_ARGS__); }
+#define SBF_DEBUG(...)           \
+    { /*GPS_WARN(__VA_ARGS__);*/ \
+    }
 
 GPSDriverSBF::GPSDriverSBF(GPSCallbackPtr callback, void *callback_user, struct sensor_gps_s *gps_position,
-			   satellite_info_s *satellite_info, float heading_offset, float pitch_offset)
-	: GPSBaseStationSupport(callback, callback_user), _gps_position(gps_position), _satellite_info(satellite_info),
-	  _heading_offset(heading_offset), _pitch_offset(pitch_offset)
-{
-	decodeInit();
+                           satellite_info_s *satellite_info, float heading_offset, float pitch_offset) :
+    GPSBaseStationSupport(callback, callback_user), _gps_position(gps_position), _satellite_info(satellite_info),
+    _heading_offset(heading_offset), _pitch_offset(pitch_offset) {
+    decodeInit();
 }
 
-GPSDriverSBF::~GPSDriverSBF()
-{
-	delete _rtcm_parsing;
+GPSDriverSBF::~GPSDriverSBF() {
+    delete _rtcm_parsing;
 }
 
-int GPSDriverSBF::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	_configured = false;
-
-	setBaudrate(SBF_TX_CFG_PRT_BAUDRATE);
-	baudrate = SBF_TX_CFG_PRT_BAUDRATE;
-	_output_mode = config.output_mode;
+int GPSDriverSBF::configure(unsigned &baudrate, const GPSConfig &config) {
+    _configured = false;
 
-	if (_output_mode != OutputMode::RTCM) {
-		sendMessage(SBF_CONFIG_FORCE_INPUT);
-	}
+    setBaudrate(SBF_TX_CFG_PRT_BAUDRATE);
+    baudrate     = SBF_TX_CFG_PRT_BAUDRATE;
+    _output_mode = config.output_mode;
 
-	// flush input and wait for at least 50 ms silence
-	decodeInit();
-	receive(50);
-	decodeInit();
+    if (_output_mode != OutputMode::RTCM) {
+        sendMessage(SBF_CONFIG_FORCE_INPUT);
+    }
 
-	char buf[GPS_READ_BUFFER_SIZE];
-	char com_port[5] {};
+    // flush input and wait for at least 50 ms silence
+    decodeInit();
+    receive(50);
+    decodeInit();
 
-	size_t offset = 1;
-	bool response_detected = false;
-	gps_abstime time_started = gps_absolute_time();
-	sendMessage("\n\r");
+    char buf[GPS_READ_BUFFER_SIZE];
+    char com_port[5]{};
 
-	// Read buffer to get the COM port
-	do {
-		--offset; //overwrite the null-char
-		int ret = read(reinterpret_cast<uint8_t *>(buf) + offset, sizeof(buf) - offset - 1, SBF_CONFIG_TIMEOUT);
+    size_t      offset            = 1;
+    bool        response_detected = false;
+    gps_abstime time_started      = gps_absolute_time();
+    sendMessage("\n\r");
 
-		if (ret < 0) {
-			// something went wrong when polling or reading
-			SBF_WARN("sbf poll_or_read err");
-			return ret;
+    // Read buffer to get the COM port
+    do {
+        --offset; //overwrite the null-char
+        int ret = read(reinterpret_cast<uint8_t *>(buf) + offset, sizeof(buf) - offset - 1, SBF_CONFIG_TIMEOUT);
 
-		}
+        if (ret < 0) {
+            // something went wrong when polling or reading
+            SBF_WARN("sbf poll_or_read err");
+            return ret;
+        }
 
-		offset += ret;
-		buf[offset++] = '\0';
+        offset        += ret;
+        buf[offset++]  = '\0';
 
 
-		char *p = strstr(buf, ">");
+        char *p = strstr(buf, ">");
 
-		if (p) { //check if the length of the com port == 4 and contains a > sign
-			for (int i = 0; i < 4; i++) {
-				com_port[i] = buf[i];
-			}
+        if (p) { //check if the length of the com port == 4 and contains a > sign
+            for (int i = 0; i < 4; i++) {
+                com_port[i] = buf[i];
+            }
 
-			response_detected = true;
-		}
+            response_detected = true;
+        }
 
-		if (offset >= sizeof(buf)) {
-			offset = 1;
-		}
+        if (offset >= sizeof(buf)) {
+            offset = 1;
+        }
 
-	} while (time_started + 1000 * SBF_CONFIG_TIMEOUT > gps_absolute_time() && !response_detected);
+    } while (time_started + 1000 * SBF_CONFIG_TIMEOUT > gps_absolute_time() && !response_detected);
 
-	if (response_detected) {
-		SBF_INFO("Septentrio GNSS receiver COM port: %s", com_port);
-		response_detected = false; // for future use
+    if (response_detected) {
+        SBF_INFO("Septentrio GNSS receiver COM port: %s", com_port);
+        response_detected = false; // for future use
 
-	} else {
-		SBF_WARN("No COM port detected")
-		return -1;
-	}
+    } else {
+        SBF_WARN("No COM port detected")
+        return -1;
+    }
 
-	// Delete all sbf outputs on current COM port to remove clutter data
-	char msg[MSG_SIZE];
-	snprintf(msg, sizeof(msg), SBF_CONFIG_RESET, com_port);
+    // Delete all sbf outputs on current COM port to remove clutter data
+    char msg[MSG_SIZE];
+    snprintf(msg, sizeof(msg), SBF_CONFIG_RESET, com_port);
 
-	if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
-		return -1; // connection and/or baudrate detection failed
-	}
+    if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
+        return -1; // connection and/or baudrate detection failed
+    }
 
-	// Set baut rate
-	snprintf(msg, sizeof(msg), SBF_CONFIG_BAUDRATE, com_port, baudrate);
+    // Set baut rate
+    snprintf(msg, sizeof(msg), SBF_CONFIG_BAUDRATE, com_port, baudrate);
 
-	if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
-		SBF_DEBUG("Connection and/or baudrate detection failed (SBF_CONFIG_BAUDRATE)");
-		return -1; // connection and/or baudrate detection failed
-	}
+    if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
+        SBF_DEBUG("Connection and/or baudrate detection failed (SBF_CONFIG_BAUDRATE)");
+        return -1; // connection and/or baudrate detection failed
+    }
 
-	// Flush input and wait for at least 50 ms silence
-	decodeInit();
-	receive(50);
-	decodeInit();
+    // Flush input and wait for at least 50 ms silence
+    decodeInit();
+    receive(50);
+    decodeInit();
 
 
-	// At this point we have correct baudrate on both ends
-	SBF_DEBUG("Correct baud rate on both ends");
+    // At this point we have correct baudrate on both ends
+    SBF_DEBUG("Correct baud rate on both ends");
 
-	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
-	snprintf(msg, sizeof(msg), SBF_DATA_IO, com_port);
+    // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
+    snprintf(msg, sizeof(msg), SBF_DATA_IO, com_port);
 
-	if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
-		return -1;
-	}
+    if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
+        return -1;
+    }
 
-	// Specify the offsets that the receiver applies to the computed attitude angles.
-	snprintf(msg, sizeof(msg), SBF_CONFIG_ATTITUDE_OFFSET, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
+    // Specify the offsets that the receiver applies to the computed attitude angles.
+    snprintf(msg, sizeof(msg), SBF_CONFIG_ATTITUDE_OFFSET, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
 
-	if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
-		return -1;
-	}
+    if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
+        return -1;
+    }
 
-	// Set the type of dynamics the GNSS antenna is subjected to.
-	if (_output_mode != OutputMode::RTCM) {
-		if (_dynamic_model < 6) {
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "low");
+    // Set the type of dynamics the GNSS antenna is subjected to.
+    if (_output_mode != OutputMode::RTCM) {
+        if (_dynamic_model < 6) {
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "low");
 
-		} else if (_dynamic_model < 7) {
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "moderate");
+        } else if (_dynamic_model < 7) {
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "moderate");
 
-		} else if (_dynamic_model < 8) {
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "high");
+        } else if (_dynamic_model < 8) {
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "high");
 
-		} else {
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "max");
-		}
+        } else {
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "max");
+        }
 
-		sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
-	}
+        sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
+    }
 
-	decodeInit();
-	receive(50);
-	decodeInit();
+    decodeInit();
+    receive(50);
+    decodeInit();
 
-	// Output a set of SBF blocks on a given connection at a regular interval.
-	int i = 0;
-	snprintf(msg, sizeof(msg), SBF_CONFIG, com_port);
+    // Output a set of SBF blocks on a given connection at a regular interval.
+    int i = 0;
+    snprintf(msg, sizeof(msg), SBF_CONFIG, com_port);
 
-	do {
-		++i;
+    do {
+        ++i;
 
-		if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
-			if (i >= 5) {
-				return -1; // connection and/or baudrate detection failed
-			}
+        if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) {
+            if (i >= 5) {
+                return -1; // connection and/or baudrate detection failed
+            }
 
-		} else {
-			response_detected = true;
-		}
-	} while (i < 5 && !response_detected);
+        } else {
+            response_detected = true;
+        }
+    } while (i < 5 && !response_detected);
 
 
+    if (_output_mode == OutputMode::RTCM) {
+        if (_base_settings.type == BaseSettingsType::fixed_position) {
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_COORDINATES,
+                     _base_settings.settings.fixed_position.latitude,
+                     _base_settings.settings.fixed_position.longitude,
+                     static_cast<double>(_base_settings.settings.fixed_position.altitude));
+            sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
+            snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_OFFSET, 0.0, 0.0, 0.0);
+            sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
+            sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC1, SBF_CONFIG_TIMEOUT);
+            sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC2, SBF_CONFIG_TIMEOUT);
+        }
+    }
 
-	if (_output_mode == OutputMode::RTCM) {
-		if (_base_settings.type == BaseSettingsType::fixed_position) {
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_COORDINATES,
-				 _base_settings.settings.fixed_position.latitude,
-				 _base_settings.settings.fixed_position.longitude,
-				 static_cast<double>(_base_settings.settings.fixed_position.altitude));
-			sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
-			snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_OFFSET, 0.0, 0.0, 0.0);
-			sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT);
-			sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC1, SBF_CONFIG_TIMEOUT);
-			sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC2, SBF_CONFIG_TIMEOUT);
-		}
-	}
-
-	_configured = true;
-	return 0;
+    _configured = true;
+    return 0;
 }
 
-bool GPSDriverSBF::sendMessage(const char *msg)
-{
-	// Send message
-	SBF_DEBUG("Send MSG: %s", msg);
-	int length = static_cast<int>(strlen(msg));
+bool GPSDriverSBF::sendMessage(const char *msg) {
+    // Send message
+    SBF_DEBUG("Send MSG: %s", msg);
+    int length = static_cast<int>(strlen(msg));
 
-	return (write(msg, length) == length);
+    return (write(msg, length) == length);
 }
 
-bool GPSDriverSBF::sendMessageAndWaitForAck(const char *msg, const int timeout)
-{
-	SBF_DEBUG("Send MSG: %s", msg);
+bool GPSDriverSBF::sendMessageAndWaitForAck(const char *msg, const int timeout) {
+    SBF_DEBUG("Send MSG: %s", msg);
 
-	// Send message
-	int length = static_cast<int>(strlen(msg));
+    // Send message
+    int length = static_cast<int>(strlen(msg));
 
-	if (write(msg, length) != length) {
-		return false;
-	}
+    if (write(msg, length) != length) {
+        return false;
+    }
 
-	// Wait for acknowledge
-	// For all valid set -, get - and exe -commands, the first line of the reply is an exact copy
-	// of the command as entered by the user, preceded with "$R:"
-	char buf[GPS_READ_BUFFER_SIZE];
-	size_t offset = 1;
-	gps_abstime time_started = gps_absolute_time();
+    // Wait for acknowledge
+    // For all valid set -, get - and exe -commands, the first line of the reply is an exact copy
+    // of the command as entered by the user, preceded with "$R:"
+    char        buf[GPS_READ_BUFFER_SIZE];
+    size_t      offset       = 1;
+    gps_abstime time_started = gps_absolute_time();
 
-	bool found_response = false;
+    bool found_response = false;
 
-	do {
-		--offset; //overwrite the null-char
-		int ret = read(reinterpret_cast<uint8_t *>(buf) + offset, sizeof(buf) - offset - 1, timeout);
+    do {
+        --offset; //overwrite the null-char
+        int ret = read(reinterpret_cast<uint8_t *>(buf) + offset, sizeof(buf) - offset - 1, timeout);
 
-		if (ret < 0) {
-			// something went wrong when polling or reading
-			SBF_WARN("sbf poll_or_read err");
-			return false;
-		}
+        if (ret < 0) {
+            // something went wrong when polling or reading
+            SBF_WARN("sbf poll_or_read err");
+            return false;
+        }
 
-		offset += ret;
-		buf[offset++] = '\0';
+        offset        += ret;
+        buf[offset++]  = '\0';
 
-		if (!found_response && strstr(buf, "$R: ") != nullptr) {
-			//SBF_DEBUG("READ %d: %s", (int) offset, buf);
-			found_response = true;
-		}
+        if (!found_response && strstr(buf, "$R: ") != nullptr) {
+            //SBF_DEBUG("READ %d: %s", (int) offset, buf);
+            found_response = true;
+        }
 
-		if (offset >= sizeof(buf)) {
-			offset = 1;
-		}
+        if (offset >= sizeof(buf)) {
+            offset = 1;
+        }
 
-	} while (time_started + 1000 * timeout > gps_absolute_time());
+    } while (time_started + 1000 * timeout > gps_absolute_time());
 
-	SBF_DEBUG("response: %u", found_response)
-	return found_response;
+    SBF_DEBUG("response: %u", found_response)
+    return found_response;
 }
 
 // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
-int GPSDriverSBF::receive(unsigned timeout)
-{
-	// Do not receive messages until we're configured
-	if (!_configured) {
-		gps_usleep(timeout * 1000);
-		return 0;
-	}
-
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
-
-	// timeout additional to poll
-	gps_abstime time_started = gps_absolute_time();
-
-	int handled = 0;
-
-	while (true) {
-		// Wait for only SBF_PACKET_TIMEOUT if something already received.
-		int ret = read(buf, sizeof(buf), handled ? SBF_PACKET_TIMEOUT : timeout);
-
-		if (ret < 0) {
-			// something went wrong when polling or reading
-			SBF_WARN("ubx poll_or_read err");
-			return -1;
-
-		} else {
-			SBF_DEBUG("Read %d bytes (receive)", ret);
-
-			// pass received bytes to the packet decoder
-			for (int i = 0; i < ret; i++) {
-				handled |= parseChar(buf[i]);
-				SBF_DEBUG("parsed %d: 0x%x", i, buf[i]);
-			}
-		}
-
-		if (handled > 0) {
-			return handled;
-		}
-
-		// abort after timeout if no useful packets received
-		if (time_started + timeout * 1000 < gps_absolute_time()) {
-			SBF_DEBUG("timed out, returning");
-			return -1;
-		}
-	}
+int GPSDriverSBF::receive(unsigned timeout) {
+    // Do not receive messages until we're configured
+    if (!_configured) {
+        gps_usleep(timeout * 1000);
+        return 0;
+    }
+
+    uint8_t buf[GPS_READ_BUFFER_SIZE];
+
+    // timeout additional to poll
+    gps_abstime time_started = gps_absolute_time();
+
+    int handled = 0;
+
+    while (true) {
+        // Wait for only SBF_PACKET_TIMEOUT if something already received.
+        int ret = read(buf, sizeof(buf), handled ? SBF_PACKET_TIMEOUT : timeout);
+
+        if (ret < 0) {
+            // something went wrong when polling or reading
+            SBF_WARN("ubx poll_or_read err");
+            return -1;
+
+        } else {
+            SBF_DEBUG("Read %d bytes (receive)", ret);
+
+            // pass received bytes to the packet decoder
+            for (int i = 0; i < ret; i++) {
+                handled |= parseChar(buf[i]);
+                SBF_DEBUG("parsed %d: 0x%x", i, buf[i]);
+            }
+        }
+
+        if (handled > 0) {
+            return handled;
+        }
+
+        // abort after timeout if no useful packets received
+        if (time_started + timeout * 1000 < gps_absolute_time()) {
+            SBF_DEBUG("timed out, returning");
+            return -1;
+        }
+    }
 }
 
 // 0 = decoding, 1 = message handled, 2 = sat info message handled
-int GPSDriverSBF::parseChar(const uint8_t b)
-{
-	int ret = 0;
-
-	switch (_decode_state) {
-
-	// Expecting Sync1
-	case SBF_DECODE_SYNC1:
-		if (b == SBF_SYNC1) { // Sync1 found --> expecting Sync2
-			SBF_TRACE_PARSER("A");
-			payloadRxAdd(b); // add a payload byte
-			_decode_state = SBF_DECODE_SYNC2;
-
-		} else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
-			SBF_TRACE_PARSER("RTCM");
-			_decode_state = SBF_DECODE_RTCM3;
-			_rtcm_parsing->addByte(b);
-		}
-
-		break;
-
-	// Expecting Sync2
-	case SBF_DECODE_SYNC2:
-		if (b == SBF_SYNC2) { // Sync2 found --> expecting CRC
-			SBF_TRACE_PARSER("B");
-			payloadRxAdd(b); // add a payload byte
-			_decode_state = SBF_DECODE_PAYLOAD;
-
-		} else { // Sync1 not followed by Sync2: reset parser
-			decodeInit();
-		}
-
-		break;
-
-	// Expecting payload
-	case SBF_DECODE_PAYLOAD: SBF_TRACE_PARSER(".");
-
-		ret = payloadRxAdd(b); // add a payload byte
-
-		if (ret < 0) {
-			// payload not handled, discard message
-			ret = 0;
-			decodeInit();
-
-		} else if (ret > 0) {
-			ret = payloadRxDone(); // finish payload processing
-			decodeInit();
-
-		} else {
-			// expecting more payload, stay in state SBF_DECODE_PAYLOAD
-			ret = 0;
-
-		}
-
-		break;
-
-	case SBF_DECODE_RTCM3:
-		if (_rtcm_parsing->addByte(b)) {
-			SBF_DEBUG("got RTCM message with length %i", (int) _rtcm_parsing->messageLength());
-			gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
-			decodeInit();
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
+int GPSDriverSBF::parseChar(const uint8_t b) {
+    int ret = 0;
+
+    switch (_decode_state) {
+    // Expecting Sync1
+    case SBF_DECODE_SYNC1:
+        if (b == SBF_SYNC1) { // Sync1 found --> expecting Sync2
+            SBF_TRACE_PARSER("A");
+            payloadRxAdd(b);  // add a payload byte
+            _decode_state = SBF_DECODE_SYNC2;
+
+        } else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
+            SBF_TRACE_PARSER("RTCM");
+            _decode_state = SBF_DECODE_RTCM3;
+            _rtcm_parsing->addByte(b);
+        }
+
+        break;
+
+    // Expecting Sync2
+    case SBF_DECODE_SYNC2:
+        if (b == SBF_SYNC2) { // Sync2 found --> expecting CRC
+            SBF_TRACE_PARSER("B");
+            payloadRxAdd(b);  // add a payload byte
+            _decode_state = SBF_DECODE_PAYLOAD;
+
+        } else { // Sync1 not followed by Sync2: reset parser
+            decodeInit();
+        }
+
+        break;
+
+    // Expecting payload
+    case SBF_DECODE_PAYLOAD:
+        SBF_TRACE_PARSER(".");
+
+        ret = payloadRxAdd(b); // add a payload byte
+
+        if (ret < 0) {
+            // payload not handled, discard message
+            ret = 0;
+            decodeInit();
+
+        } else if (ret > 0) {
+            ret = payloadRxDone(); // finish payload processing
+            decodeInit();
+
+        } else {
+            // expecting more payload, stay in state SBF_DECODE_PAYLOAD
+            ret = 0;
+        }
+
+        break;
+
+    case SBF_DECODE_RTCM3:
+        if (_rtcm_parsing->addByte(b)) {
+            SBF_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
+            gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
+            decodeInit();
+        }
+
+        break;
+
+    default:
+        break;
+    }
+
+    return ret;
 }
 
 /**
  * Add payload rx byte
  */
 // -1 = error, 0 = ok, 1 = payload completed
-int GPSDriverSBF::payloadRxAdd(const uint8_t b)
-{
-	int ret = 0;
-	uint8_t *p_buf = reinterpret_cast<uint8_t *>(&_buf);
+int GPSDriverSBF::payloadRxAdd(const uint8_t b) {
+    int      ret   = 0;
+    uint8_t *p_buf = reinterpret_cast<uint8_t *>(&_buf);
 
-	p_buf[_rx_payload_index++] = b;
+    p_buf[_rx_payload_index++] = b;
 
-	if ((_rx_payload_index > 7 && _rx_payload_index >= _buf.length) || _rx_payload_index >= sizeof(_buf)) {
-		ret = 1; // payload received completely
-	}
+    if ((_rx_payload_index > 7 && _rx_payload_index >= _buf.length) || _rx_payload_index >= sizeof(_buf)) {
+        ret = 1; // payload received completely
+    }
 
-	return ret;
+    return ret;
 }
 
 /**
  * Calculate buffer CRC16
  */
-uint16_t crc16(const uint8_t *data_p, uint32_t length)
-{
-	uint8_t x;
-	uint16_t crc = 0;
-
-	while (length--) {
-		x = crc >> 8 ^ *data_p++;
-		x ^= x >> 4;
-		crc = static_cast<uint16_t>((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
-	}
-
-	return crc;
+uint16_t crc16(const uint8_t *data_p, uint32_t length) {
+    uint8_t  x;
+    uint16_t crc = 0;
+
+    while (length--) {
+        x    = crc >> 8 ^ *data_p++;
+        x   ^= x >> 4;
+        crc  = static_cast<uint16_t>((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
+    }
+
+    return crc;
 }
 
 /**
  * Finish payload rx
  */
 // 0 = no message handled, 1 = message handled, 2 = sat info message handled
-int GPSDriverSBF::payloadRxDone()
-{
-	int ret = 0;
+int GPSDriverSBF::payloadRxDone() {
+    int ret = 0;
 #ifndef NO_MKTIME
-	struct tm timeinfo;
-	time_t epoch;
+    struct tm timeinfo;
+    time_t    epoch;
 #endif
 
-	if (_buf.length <= 4 ||
-	    _buf.length > _rx_payload_index ||
-	    _buf.crc16 != crc16(reinterpret_cast<uint8_t *>(&_buf) + 4, _buf.length - 4)) {
-		return 0;
-	}
-
-	// handle message
-	switch (_buf.msg_id) {
-	case SBF_ID_PVTGeodetic: SBF_TRACE_RXMSG("Rx PVTGeodetic");
-		_msg_status |= 1;
-
-		if (_buf.payload_pvt_geodetic.mode_type < 1) {
-			_gps_position->fix_type = 1;
-
-		} else if (_buf.payload_pvt_geodetic.mode_type == 6) {
-			_gps_position->fix_type = 4;
-
-		} else if (_buf.payload_pvt_geodetic.mode_type == 5 || _buf.payload_pvt_geodetic.mode_type == 8) {
-			_gps_position->fix_type = 5;
-
-		} else if (_buf.payload_pvt_geodetic.mode_type == 4 || _buf.payload_pvt_geodetic.mode_type == 7) {
-			_gps_position->fix_type = 6;
-
-		} else {
-			_gps_position->fix_type = 3;
-		}
-
-		// Check fix and error code
-		_gps_position->vel_ned_valid = _gps_position->fix_type > 1 && _buf.payload_pvt_geodetic.error == 0;
-
-		// Check boundaries and invalidate GPS velocities
-		// We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values
-		if (fabsf(_buf.payload_pvt_geodetic.vn) > 600.0f || fabsf(_buf.payload_pvt_geodetic.ve) > 600.0f ||
-		    fabsf(_buf.payload_pvt_geodetic.vu) > 600.0f) {
-			_gps_position->vel_ned_valid = false;
-		}
-
-		// Check boundaries and invalidate position
-		// We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values
-		if (fabs(_buf.payload_pvt_geodetic.latitude) > (double) M_PI_2_F ||
-		    fabs(_buf.payload_pvt_geodetic.longitude) > (double) M_PI_F ||
-		    fabs(_buf.payload_pvt_geodetic.height) > DNU ||
-		    fabsf(_buf.payload_pvt_geodetic.undulation) > (float) DNU) {
-			_gps_position->fix_type = 0;
-		}
-
-		if (_buf.payload_pvt_geodetic.nr_sv < 255) {  // 255 = do not use value
-			_gps_position->satellites_used = _buf.payload_pvt_geodetic.nr_sv;
-
-			if (_satellite_info) {
-				// Only fill in the satellite count for now (we could use the ChannelStatus message for the
-				// other data, but it's really large: >800B)
-				_satellite_info->timestamp = gps_absolute_time();
-				_satellite_info->count = _gps_position->satellites_used;
-				ret = 2;
-			}
-
-		} else {
-			_gps_position->satellites_used = 0;
-		}
-
-		_gps_position->lat = static_cast<int>(round(_buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG * 1e7));
-		_gps_position->lon = static_cast<int>(round(_buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG * 1e7));
-		_gps_position->alt_ellipsoid = static_cast<int>(round(_buf.payload_pvt_geodetic.height * 1000));
-		_gps_position->alt = static_cast<int>(round((_buf.payload_pvt_geodetic.height - static_cast<double>
-						      (_buf.payload_pvt_geodetic.undulation)) * 1000));
-
-		/* H and V accuracy are reported in 2DRMS, but based off the uBlox reporting we expect RMS.
+    if (_buf.length <= 4 || _buf.length > _rx_payload_index || _buf.crc16 != crc16(reinterpret_cast<uint8_t *>(&_buf) + 4, _buf.length - 4)) {
+        return 0;
+    }
+
+    // handle message
+    switch (_buf.msg_id) {
+    case SBF_ID_PVTGeodetic:
+        SBF_TRACE_RXMSG("Rx PVTGeodetic");
+        _msg_status |= 1;
+
+        if (_buf.payload_pvt_geodetic.mode_type < 1) {
+            _gps_position->fix_type = 1;
+
+        } else if (_buf.payload_pvt_geodetic.mode_type == 6) {
+            _gps_position->fix_type = 4;
+
+        } else if (_buf.payload_pvt_geodetic.mode_type == 5 || _buf.payload_pvt_geodetic.mode_type == 8) {
+            _gps_position->fix_type = 5;
+
+        } else if (_buf.payload_pvt_geodetic.mode_type == 4 || _buf.payload_pvt_geodetic.mode_type == 7) {
+            _gps_position->fix_type = 6;
+
+        } else {
+            _gps_position->fix_type = 3;
+        }
+
+        // Check fix and error code
+        _gps_position->vel_ned_valid = _gps_position->fix_type > 1 && _buf.payload_pvt_geodetic.error == 0;
+
+        // Check boundaries and invalidate GPS velocities
+        // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values
+        if (fabsf(_buf.payload_pvt_geodetic.vn) > 600.0f || fabsf(_buf.payload_pvt_geodetic.ve) > 600.0f || fabsf(_buf.payload_pvt_geodetic.vu) > 600.0f) {
+            _gps_position->vel_ned_valid = false;
+        }
+
+        // Check boundaries and invalidate position
+        // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values
+        if (fabs(_buf.payload_pvt_geodetic.latitude) > (double)M_PI_2_F || fabs(_buf.payload_pvt_geodetic.longitude) > (double)M_PI_F || fabs(_buf.payload_pvt_geodetic.height) > DNU || fabsf(_buf.payload_pvt_geodetic.undulation) > (float)DNU) {
+            _gps_position->fix_type = 0;
+        }
+
+        if (_buf.payload_pvt_geodetic.nr_sv < 255) { // 255 = do not use value
+            _gps_position->satellites_used = _buf.payload_pvt_geodetic.nr_sv;
+
+            if (_satellite_info) {
+                // Only fill in the satellite count for now (we could use the ChannelStatus message for the
+                // other data, but it's really large: >800B)
+                _satellite_info->timestamp = gps_absolute_time();
+                _satellite_info->count     = _gps_position->satellites_used;
+                ret                        = 2;
+            }
+
+        } else {
+            _gps_position->satellites_used = 0;
+        }
+
+        _gps_position->lat           = static_cast<int>(round(_buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG * 1e7));
+        _gps_position->lon           = static_cast<int>(round(_buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG * 1e7));
+        _gps_position->alt_ellipsoid = static_cast<int>(round(_buf.payload_pvt_geodetic.height * 1000));
+        _gps_position->alt           = static_cast<int>(round((_buf.payload_pvt_geodetic.height - static_cast<double>(_buf.payload_pvt_geodetic.undulation)) * 1000));
+
+        /* H and V accuracy are reported in 2DRMS, but based off the uBlox reporting we expect RMS.
 		 * Devide by 100 from cm to m and in addition divide by 2 to get RMS. */
-		_gps_position->eph = static_cast<float>(_buf.payload_pvt_geodetic.h_accuracy) / 200.0f;
-		_gps_position->epv = static_cast<float>(_buf.payload_pvt_geodetic.v_accuracy) / 200.0f;
+        _gps_position->eph = static_cast<float>(_buf.payload_pvt_geodetic.h_accuracy) / 200.0f;
+        _gps_position->epv = static_cast<float>(_buf.payload_pvt_geodetic.v_accuracy) / 200.0f;
 
-		_gps_position->vel_n_m_s = static_cast<float>(_buf.payload_pvt_geodetic.vn);
-		_gps_position->vel_e_m_s = static_cast<float>(_buf.payload_pvt_geodetic.ve);
-		_gps_position->vel_d_m_s = -1.0f * static_cast<float>(_buf.payload_pvt_geodetic.vu);
-		_gps_position->vel_m_s = sqrtf(_gps_position->vel_n_m_s * _gps_position->vel_n_m_s +
-					       _gps_position->vel_e_m_s * _gps_position->vel_e_m_s);
+        _gps_position->vel_n_m_s = static_cast<float>(_buf.payload_pvt_geodetic.vn);
+        _gps_position->vel_e_m_s = static_cast<float>(_buf.payload_pvt_geodetic.ve);
+        _gps_position->vel_d_m_s = -1.0f * static_cast<float>(_buf.payload_pvt_geodetic.vu);
+        _gps_position->vel_m_s   = sqrtf(_gps_position->vel_n_m_s * _gps_position->vel_n_m_s + _gps_position->vel_e_m_s * _gps_position->vel_e_m_s);
 
-		_gps_position->cog_rad = static_cast<float>(_buf.payload_pvt_geodetic.cog) * M_DEG_TO_RAD_F;
-		_gps_position->c_variance_rad = 1.0f * M_DEG_TO_RAD_F;
+        _gps_position->cog_rad        = static_cast<float>(_buf.payload_pvt_geodetic.cog) * M_DEG_TO_RAD_F;
+        _gps_position->c_variance_rad = 1.0f * M_DEG_TO_RAD_F;
 
-		// _buf.payload_pvt_geodetic.cog is set to -2*10^10 for velocities below 0.1m/s
-		if (_buf.payload_pvt_geodetic.cog > 360.0f) {
-			_buf.payload_pvt_geodetic.cog = NAN;
-		}
+        // _buf.payload_pvt_geodetic.cog is set to -2*10^10 for velocities below 0.1m/s
+        if (_buf.payload_pvt_geodetic.cog > 360.0f) {
+            _buf.payload_pvt_geodetic.cog = NAN;
+        }
 
-		_gps_position->time_utc_usec = 0;
+        _gps_position->time_utc_usec = 0;
 #ifndef NO_MKTIME
-		/* convert to unix timestamp */
-		memset(&timeinfo, 0, sizeof(timeinfo));
+        /* convert to unix timestamp */
+        memset(&timeinfo, 0, sizeof(timeinfo));
 
-		timeinfo.tm_year = 1980 - 1900;
-		timeinfo.tm_mon = 0;
-		timeinfo.tm_mday = 6 + _buf.WNc * 7;
-		timeinfo.tm_hour = 0;
-		timeinfo.tm_min = 0;
-		timeinfo.tm_sec = _buf.TOW / 1000;
+        timeinfo.tm_year = 1980 - 1900;
+        timeinfo.tm_mon  = 0;
+        timeinfo.tm_mday = 6 + _buf.WNc * 7;
+        timeinfo.tm_hour = 0;
+        timeinfo.tm_min  = 0;
+        timeinfo.tm_sec  = _buf.TOW / 1000;
 
-		epoch = mktime(&timeinfo);
+        epoch = mktime(&timeinfo);
 
-		if (epoch > GPS_EPOCH_SECS) {
-			// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-			// and control its drift. Since we rely on the HRT for our monotonic
-			// clock, updating it from time to time is safe.
+        if (epoch > GPS_EPOCH_SECS) {
+            // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+            // and control its drift. Since we rely on the HRT for our monotonic
+            // clock, updating it from time to time is safe.
 
-			timespec ts;
-			memset(&ts, 0, sizeof(ts));
-			ts.tv_sec = epoch;
-			ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000;
-			setClock(ts);
+            timespec ts;
+            memset(&ts, 0, sizeof(ts));
+            ts.tv_sec  = epoch;
+            ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000;
+            setClock(ts);
 
-			_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-			_gps_position->time_utc_usec += (_buf.TOW % 1000) * 1000;
-		}
+            _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+            _gps_position->time_utc_usec += (_buf.TOW % 1000) * 1000;
+        }
 
 #endif
-		_gps_position->timestamp = gps_absolute_time();
-		_last_timestamp_time = _gps_position->timestamp;
-		_rate_count_vel++;
-		_rate_count_lat_lon++;
-		ret |= (_msg_status == 7) ? 1 : 0;
-		//SBF_DEBUG("PVTGeodetic handled");
-		break;
-
-	case SBF_ID_VelCovGeodetic: SBF_TRACE_RXMSG("Rx VelCovGeodetic");
-		_msg_status |= 2;
-		_gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_ve_ve;
-
-		if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vn_vn) {
-			_gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vn_vn;
-		}
-
-		if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vu_vu) {
-			_gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vu_vu;
-		}
-
-		//SBF_DEBUG("VelCovGeodetic handled");
-		break;
-
-	case SBF_ID_DOP: SBF_TRACE_RXMSG("Rx DOP");
-		_msg_status |= 4;
-		_gps_position->hdop = _buf.payload_dop.hDOP * 0.01f;
-		_gps_position->vdop = _buf.payload_dop.vDOP * 0.01f;
-		//SBF_DEBUG("DOP handled");
-		break;
-
-	case SBF_ID_AttEuler: SBF_TRACE_RXMSG("Rx AttEuler");
-
-		if (!_buf.payload_att_euler.error_not_requested) {
-
-			int error_aux1 = _buf.payload_att_euler.error_aux1;
-			int error_aux2 = _buf.payload_att_euler.error_aux2;
-
-			// SBF_DEBUG("Mode: %u", _buf.payload_att_euler.mode)
-			if (error_aux1 == 0 && error_aux2 == 0) {
-				float heading = _buf.payload_att_euler.heading;
-				heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
-
-
-				if (heading > M_PI_F) {
-					heading -= 2.f * M_PI_F; // final range is [-pi, pi]
-				}
-
-				_gps_position->heading = heading;
-				// SBF_DEBUG("Heading: %.3f rad", (double) _gps_position->heading)
-				//SBF_DEBUG("AttEuler handled");
-
-			} else if (error_aux1 != 0) {
-				//SBF_DEBUG("Error code for Main-Aux1 baseline: Not enough measurements");
-			} else if (error_aux2 != 0) {
-				//SBF_DEBUG("Error code for Main-Aux2 baseline: Not enough measurements");
-			}
-		} else {
-			//SBF_DEBUG("AttEuler: attitude not requested by user");
-		}
-
-
-		break;
-
-	case SBF_ID_AttCovEuler: SBF_TRACE_RXMSG("Rx AttCovEuler");
-
-		if (!_buf.payload_att_cov_euler.error_not_requested) {
-			int error_aux1 = _buf.payload_att_cov_euler.error_aux1;
-			int error_aux2 = _buf.payload_att_cov_euler.error_aux2;
-
-			if (error_aux1 == 0 && error_aux2 == 0) {
-				float heading_acc = _buf.payload_att_cov_euler.cov_headhead;
-				heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
-				_gps_position->heading_accuracy = heading_acc;
-				// SBF_DEBUG("Heading-Accuracy: %.3f rad", (double) _gps_position->heading_accuracy)
-				//SBF_DEBUG("AttCovEuler handled");
-
-			} else if (error_aux1 != 0) {
-				//SBF_DEBUG("Error code for Main-Aux1 baseline: %u: Not enough measurements", error_aux1);
-			} else if (error_aux2 != 0) {
-				//SBF_DEBUG("Error code for Main-Aux2 baseline: %u: Not enough measurements", error_aux2);
-			}
-		} else {
-			//SBF_DEBUG("AttCovEuler: attitude not requested by user");
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	if (ret > 0) {
-		_gps_position->timestamp_time_relative = static_cast<int32_t>(_last_timestamp_time - _gps_position->timestamp);
-	}
-
-	if (ret == 1) {
-		_msg_status &= ~1;
-	}
-
-	return ret;
+        _gps_position->timestamp = gps_absolute_time();
+        _last_timestamp_time     = _gps_position->timestamp;
+        _rate_count_vel++;
+        _rate_count_lat_lon++;
+        ret |= (_msg_status == 7) ? 1 : 0;
+        //SBF_DEBUG("PVTGeodetic handled");
+        break;
+
+    case SBF_ID_VelCovGeodetic:
+        SBF_TRACE_RXMSG("Rx VelCovGeodetic");
+        _msg_status                   |= 2;
+        _gps_position->s_variance_m_s  = _buf.payload_vel_col_geodetic.cov_ve_ve;
+
+        if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vn_vn) {
+            _gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vn_vn;
+        }
+
+        if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vu_vu) {
+            _gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vu_vu;
+        }
+
+        //SBF_DEBUG("VelCovGeodetic handled");
+        break;
+
+    case SBF_ID_DOP:
+        SBF_TRACE_RXMSG("Rx DOP");
+        _msg_status         |= 4;
+        _gps_position->hdop  = _buf.payload_dop.hDOP * 0.01f;
+        _gps_position->vdop  = _buf.payload_dop.vDOP * 0.01f;
+        //SBF_DEBUG("DOP handled");
+        break;
+
+    case SBF_ID_AttEuler:
+        SBF_TRACE_RXMSG("Rx AttEuler");
+
+        if (!_buf.payload_att_euler.error_not_requested) {
+            int error_aux1 = _buf.payload_att_euler.error_aux1;
+            int error_aux2 = _buf.payload_att_euler.error_aux2;
+
+            // SBF_DEBUG("Mode: %u", _buf.payload_att_euler.mode)
+            if (error_aux1 == 0 && error_aux2 == 0) {
+                float heading  = _buf.payload_att_euler.heading;
+                heading       *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
+
+
+                if (heading > M_PI_F) {
+                    heading -= 2.f * M_PI_F; // final range is [-pi, pi]
+                }
+
+                _gps_position->heading = heading;
+                // SBF_DEBUG("Heading: %.3f rad", (double) _gps_position->heading)
+                //SBF_DEBUG("AttEuler handled");
+
+            } else if (error_aux1 != 0) {
+                //SBF_DEBUG("Error code for Main-Aux1 baseline: Not enough measurements");
+            } else if (error_aux2 != 0) {
+                //SBF_DEBUG("Error code for Main-Aux2 baseline: Not enough measurements");
+            }
+        } else {
+            //SBF_DEBUG("AttEuler: attitude not requested by user");
+        }
+
+
+        break;
+
+    case SBF_ID_AttCovEuler:
+        SBF_TRACE_RXMSG("Rx AttCovEuler");
+
+        if (!_buf.payload_att_cov_euler.error_not_requested) {
+            int error_aux1 = _buf.payload_att_cov_euler.error_aux1;
+            int error_aux2 = _buf.payload_att_cov_euler.error_aux2;
+
+            if (error_aux1 == 0 && error_aux2 == 0) {
+                float heading_acc                = _buf.payload_att_cov_euler.cov_headhead;
+                heading_acc                     *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
+                _gps_position->heading_accuracy  = heading_acc;
+                // SBF_DEBUG("Heading-Accuracy: %.3f rad", (double) _gps_position->heading_accuracy)
+                //SBF_DEBUG("AttCovEuler handled");
+
+            } else if (error_aux1 != 0) {
+                //SBF_DEBUG("Error code for Main-Aux1 baseline: %u: Not enough measurements", error_aux1);
+            } else if (error_aux2 != 0) {
+                //SBF_DEBUG("Error code for Main-Aux2 baseline: %u: Not enough measurements", error_aux2);
+            }
+        } else {
+            //SBF_DEBUG("AttCovEuler: attitude not requested by user");
+        }
+
+        break;
+
+    default:
+        break;
+    }
+
+    if (ret > 0) {
+        _gps_position->timestamp_time_relative = static_cast<int32_t>(_last_timestamp_time - _gps_position->timestamp);
+    }
+
+    if (ret == 1) {
+        _msg_status &= ~1;
+    }
+
+    return ret;
 }
 
-void GPSDriverSBF::decodeInit()
-{
-	_decode_state = SBF_DECODE_SYNC1;
-	_rx_payload_index = 0;
+void GPSDriverSBF::decodeInit() {
+    _decode_state     = SBF_DECODE_SYNC1;
+    _rx_payload_index = 0;
 
-	if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
-		if (!_rtcm_parsing) {
-			_rtcm_parsing = new RTCMParsing();
-		}
+    if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) {
+        if (!_rtcm_parsing) {
+            _rtcm_parsing = new RTCMParsing();
+        }
 
-		if (_rtcm_parsing) {
-			_rtcm_parsing->reset();
-		}
-	}
+        if (_rtcm_parsing) {
+            _rtcm_parsing->reset();
+        }
+    }
 }
 
-int GPSDriverSBF::reset(GPSRestartType restart_type)
-{
-	bool res = false;
+int GPSDriverSBF::reset(GPSRestartType restart_type) {
+    bool res = false;
 
-	switch (restart_type) {
-	case GPSRestartType::Hot:
-		res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_HOT, SBF_CONFIG_TIMEOUT);
-		break;
+    switch (restart_type) {
+    case GPSRestartType::Hot:
+        res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_HOT, SBF_CONFIG_TIMEOUT);
+        break;
 
-	case GPSRestartType::Warm:
-		res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_WARM, SBF_CONFIG_TIMEOUT);
-		break;
+    case GPSRestartType::Warm:
+        res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_WARM, SBF_CONFIG_TIMEOUT);
+        break;
 
-	case GPSRestartType::Cold:
-		res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_COLD, SBF_CONFIG_TIMEOUT);
-		break;
+    case GPSRestartType::Cold:
+        res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_COLD, SBF_CONFIG_TIMEOUT);
+        break;
 
-	default:
-		break;
-	}
+    default:
+        break;
+    }
 
-	return (res) ? 0 : -2;
+    return (res) ? 0 : -2;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/sbf.h b/apps/peripheral/sensor/gnss/gps/devices/src/sbf.h
index 0b1aefed1e..1f8a1c78ed 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/sbf.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/sbf.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file sbf.h
@@ -49,59 +26,56 @@
 #include "../../definitions.h"
 
 
-#define SBF_CONFIG_FORCE_INPUT "SSSSSSSSSS\n"
+#define SBF_CONFIG_FORCE_INPUT       "SSSSSSSSSS\n"
 
-#define SBF_CONFIG_BAUDRATE "setCOMSettings, %s, baud%d\n"
+#define SBF_CONFIG_BAUDRATE          "setCOMSettings, %s, baud%d\n"
 
-#define SBF_CONFIG_RESET "setSBFOutput, Stream1, %s, none, off\n"
+#define SBF_CONFIG_RESET             "setSBFOutput, Stream1, %s, none, off\n"
 
 #define SBF_CONFIG_RECEIVER_DYNAMICS "setReceiverDynamics, %s, UAV\n"
 
-#define SBF_CONFIG_ATTITUDE_OFFSET "setAttitudeOffset, %.3f, %.3f\n"
+#define SBF_CONFIG_ATTITUDE_OFFSET   "setAttitudeOffset, %.3f, %.3f\n"
 
-#define SBF_TX_CFG_PRT_BAUDRATE 115200
+#define SBF_TX_CFG_PRT_BAUDRATE      115200
 
-#define SBF_DATA_IO "setDataInOut, %s, Auto, SBF\n"
+#define SBF_DATA_IO                  "setDataInOut, %s, Auto, SBF\n"
 
-#define SBF_CONFIG "setSBFOutput, Stream1, %s, PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler, msec100\n"
+#define SBF_CONFIG                   "setSBFOutput, Stream1, %s, PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler, msec100\n"
 
 
-#define SBF_CONFIG_RTCM "" \
-	"setDataInOut, USB1, Auto, RTCMv3+SBF\n" \
-	"setPVTMode, Rover, All, auto\n" \
-	"setSatelliteTracking, All\n" \
-	"setSatelliteUsage, All\n" \
-	"setElevationMask, All, 10\n" \
-	"setReceiverDynamics, Moderate, Automotive\n" \
-	"setSBFOutput, Stream1, DSK1, Support, msec100\n" \
-	"setSBFOutput, Stream2, USB1, DOP+VelCovGeodetic, sec1\n" \
-	"setSBFOutput, Stream3, USB1, PVTGeodetic, msec200\n" \
-	"setFileNaming, DSK1, Incremental\n" \
-	"setFileNaming, DSK1, , 'px4rtcm'\n"
+#define SBF_CONFIG_RTCM              ""                                           \
+                        "setDataInOut, USB1, Auto, RTCMv3+SBF\n"                  \
+                        "setPVTMode, Rover, All, auto\n"                          \
+                        "setSatelliteTracking, All\n"                             \
+                        "setSatelliteUsage, All\n"                                \
+                        "setElevationMask, All, 10\n"                             \
+                        "setReceiverDynamics, Moderate, Automotive\n"             \
+                        "setSBFOutput, Stream1, DSK1, Support, msec100\n"         \
+                        "setSBFOutput, Stream2, USB1, DOP+VelCovGeodetic, sec1\n" \
+                        "setSBFOutput, Stream3, USB1, PVTGeodetic, msec200\n"     \
+                        "setFileNaming, DSK1, Incremental\n"                      \
+                        "setFileNaming, DSK1, , 'px4rtcm'\n"
 
 #define SBF_CONFIG_RTCM_STATIC1 "" \
-	"setReceiverDynamics, Low, Static\n"
+                                "setReceiverDynamics, Low, Static\n"
 
 #define SBF_CONFIG_RTCM_STATIC2 "" \
-	"setPVTMode, Static, , Geodetic1\n"
+                                "setPVTMode, Static, , Geodetic1\n"
 
 #define SBF_CONFIG_RTCM_STATIC_COORDINATES "" \
-	"setStaticPosGeodetic, Geodetic1, %f, %f, %f\n"
+                                           "setStaticPosGeodetic, Geodetic1, %f, %f, %f\n"
 
 #define SBF_CONFIG_RTCM_STATIC_OFFSET "" \
-	"setAntennaOffset, Main, %f, %f, %f\n"
+                                      "setAntennaOffset, Main, %f, %f, %f\n"
 
-#define SBF_CONFIG_RESET_HOT "" \
-	SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, soft, none\n"
+#define SBF_CONFIG_RESET_HOT  "" SBF_CONFIG_FORCE_INPUT "ExeResetReceiver, soft, none\n"
 
-#define SBF_CONFIG_RESET_WARM "" \
-	SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, soft, PVTData\n"
+#define SBF_CONFIG_RESET_WARM "" SBF_CONFIG_FORCE_INPUT "ExeResetReceiver, soft, PVTData\n"
 
-#define SBF_CONFIG_RESET_COLD "" \
-	SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, hard, SatData\n"
+#define SBF_CONFIG_RESET_COLD "" SBF_CONFIG_FORCE_INPUT "ExeResetReceiver, hard, SatData\n"
 
-#define SBF_SYNC1 0x24
-#define SBF_SYNC2 0x40
+#define SBF_SYNC1             0x24
+#define SBF_SYNC2             0x40
 
 /* Block IDs */
 #define SBF_ID_DOP            4001
@@ -115,7 +89,7 @@
 #pragma pack(push, 1)
 
 typedef struct {
-	uint8_t mode_type: 4;       /**< Bit field indicating the PVT mode type, as follows:
+    uint8_t  mode_type       : 4; /**< Bit field indicating the PVT mode type, as follows:
                                      0: No PVT available (the Error field indicates the cause of the absence of the PVT solution)
                                      1: Stand-Alone PVT
                                      2: Differential PVT
@@ -126,11 +100,11 @@ typedef struct {
                                      7: moving-base RTK with fixed ambiguities
                                      8: moving-base RTK with float ambiguities
                                      10:Precise Point Positioning (PPP) */
-	uint8_t mode_reserved: 2;   /**< Reserved */
-	uint8_t mode_base_fixed: 1; /**< Set if the user has entered the command setPVTMode,base,auto and the receiver
+    uint8_t  mode_reserved   : 2; /**< Reserved */
+    uint8_t  mode_base_fixed : 1; /**< Set if the user has entered the command setPVTMode,base,auto and the receiver
                                      is still in the process of determining its fixed position. */
-	uint8_t mode_2d: 1;        /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */
-	uint8_t error;             /**< PVT error code. The following values are defined:
+    uint8_t  mode_2d         : 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */
+    uint8_t  error;               /**< PVT error code. The following values are defined:
                                      0: No Error
                                      1: Not enough measurements
                                      2: Not enough ephemerides available
@@ -143,25 +117,25 @@ typedef struct {
                                      9: Base station coordinates unavailable
                                      10:Ambiguities not fixed and user requested to only output RTK-fixed positions
                                      Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */
-	double latitude;          /**< Marker latitude, from -PI/2 to +PI/2, positive North of Equator */
-	double longitude;         /**< Marker longitude, from -PI to +PI, positive East of Greenwich */
-	double height;            /**< Marker ellipsoidal height (with respect to the ellipsoid specified by Datum) */
-	float undulation;        /**< Geoid undulation computed from the global geoid model defined in
+    double   latitude;            /**< Marker latitude, from -PI/2 to +PI/2, positive North of Equator */
+    double   longitude;           /**< Marker longitude, from -PI to +PI, positive East of Greenwich */
+    double   height;              /**< Marker ellipsoidal height (with respect to the ellipsoid specified by Datum) */
+    float    undulation;          /**< Geoid undulation computed from the global geoid model defined in
                                      the document 'Technical Characteristics of the NAVSTAR GPS, NATO, June 1991' */
-	float vn;                /**< Velocity in the North direction */
-	float ve;                /**< Velocity in the East direction */
-	float vu;                /**< Velocity in the Up direction */
-	float cog;               /**< Course over ground: this is defined as the angle of the vehicle with respect
+    float    vn;                  /**< Velocity in the North direction */
+    float    ve;                  /**< Velocity in the East direction */
+    float    vu;                  /**< Velocity in the Up direction */
+    float    cog;                 /**< Course over ground: this is defined as the angle of the vehicle with respect
                                      to the local level North, ranging from 0 to 360, and increasing towards east.
                                      Set to the do-not-use value when the speed is lower than 0.1m/s. */
-	double rx_clk_bias;       /**< Receiver clock bias relative to system time reported in the Time System field.
+    double   rx_clk_bias;         /**< Receiver clock bias relative to system time reported in the Time System field.
                                      To transfer the receiver time to the system time, use: tGPS/GST=trx-RxClkBias */
-	float RxClkDrift;        /**< Receiver clock drift relative to system time (relative frequency error) */
-	uint8_t time_system;       /**< Time system of which the offset is provided in this sub-block:
+    float    RxClkDrift;          /**< Receiver clock drift relative to system time (relative frequency error) */
+    uint8_t  time_system;         /**< Time system of which the offset is provided in this sub-block:
                                      0:GPStime
                                      1:Galileotime
                                      3:GLONASStime */
-	uint8_t datum;             /**< This field defines in which datum the coordinates are expressed:
+    uint8_t  datum;               /**< This field defines in which datum the coordinates are expressed:
                                      0: WGS84/ITRS
                                      19: Datum equal to that used by the DGNSS/RTK basestation
                                      30: ETRS89(ETRF2000 realization)
@@ -171,36 +145,36 @@ typedef struct {
                                      34: GDA94(2010), Geocentric Datum of Australia (2010)
                                      250:First user-defined datum
                                      251:Second user-defined datum */
-	uint8_t nr_sv;             /**< Total number of satellites used in the PVT computation. */
-	uint8_t wa_corr_info;      /**< Bit field providing information about which wide area corrections have been applied:
+    uint8_t  nr_sv;               /**< Total number of satellites used in the PVT computation. */
+    uint8_t  wa_corr_info;        /**< Bit field providing information about which wide area corrections have been applied:
                                      Bit 0: set if orbit and satellite clock correction information is used
                                      Bit 1: set if range correction information is used
                                      Bit 2: set if ionospheric information is used
                                      Bit 3: set if orbit accuracy information is used(UERE/SISA)
                                      Bit 4: set if DO229 Precision Approach mode is active
                                      Bits 5-7: Reserved */
-	uint16_t reference_id;      /**< In case of DGPS or RTK operation, this field is to be interpreted as the base station identifier.
+    uint16_t reference_id;        /**< In case of DGPS or RTK operation, this field is to be interpreted as the base station identifier.
                                      In SBAS operation, this field is to be interpreted as the PRN of the geostationary satellite
                                      used (from 120 to 158). If multiple base stations or multiple geostationary satellites are used
                                      the value is set to 65534.*/
-	uint16_t mean_corr_age;     /**< In case of DGPS or RTK, this field is the mean age of the differential corrections.
+    uint16_t mean_corr_age;       /**< In case of DGPS or RTK, this field is the mean age of the differential corrections.
                                      In case of SBAS operation, this field is the mean age of the 'fast corrections'
                                      provided by the SBAS satellites */
-	uint32_t signal_info;       /**< Bit field indicating the type of GNSS signals having been used in the PVT computations.
+    uint32_t signal_info;         /**< Bit field indicating the type of GNSS signals having been used in the PVT computations.
                                      If a bit i is set, the signal type having index i has been used. */
-	uint8_t alert_flag;         /**< Bit field indicating integrity related information */
-
-	// Revision 1
-	uint8_t nr_bases;
-	uint16_t ppp_info;
-	// Revision 2
-	uint16_t latency;
-	uint16_t h_accuracy;
-	uint16_t v_accuracy;
+    uint8_t  alert_flag;          /**< Bit field indicating integrity related information */
+
+    // Revision 1
+    uint8_t  nr_bases;
+    uint16_t ppp_info;
+    // Revision 2
+    uint16_t latency;
+    uint16_t h_accuracy;
+    uint16_t v_accuracy;
 } sbf_payload_pvt_geodetic_t;
 
 typedef struct {
-	uint8_t mode_type: 4;       /**< Bit field indicating the PVT mode type, as follows:
+    uint8_t mode_type       : 4; /**< Bit field indicating the PVT mode type, as follows:
                                      0: No PVT available (the Error field indicates the cause of the absence of the PVT solution)
                                      1: Stand-Alone PVT
                                      2: Differential PVT
@@ -211,11 +185,11 @@ typedef struct {
                                      7: moving-base RTK with fixed ambiguities
                                      8: moving-base RTK with float ambiguities
                                      10:Precise Point Positioning (PPP) */
-	uint8_t mode_reserved: 2;  /**< Reserved */
-	uint8_t mode_base_fixed: 1;/**< Set if the user has entered the command setPVTMode,base,auto and the receiver
+    uint8_t mode_reserved   : 2; /**< Reserved */
+    uint8_t mode_base_fixed : 1; /**< Set if the user has entered the command setPVTMode,base,auto and the receiver
                                      is still in the process of determining its fixed position. */
-	uint8_t mode_2d: 1;        /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */
-	uint8_t error;             /**< PVT error code. The following values are defined:
+    uint8_t mode_2d         : 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */
+    uint8_t error;               /**< PVT error code. The following values are defined:
                                      0: No Error
                                      1: Not enough measurements
                                      2: Not enough ephemerides available
@@ -228,50 +202,49 @@ typedef struct {
                                      9: Base station coordinates unavailable
                                      10:Ambiguities not fixed and user requested to only output RTK-fixed positions
                                      Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */
-	float cov_vn_vn;            /**< Variance of the north-velocity estimate */
-	float cov_ve_ve;            /**< Variance of the east-velocity estimate */
-	float cov_vu_vu;            /**< Variance of the up - velocity estimate */
-	float cov_dt_dt;            /**< Variance of the clock drift estimate */
-	float cov_vn_ve;            /**< Covariance between the north - and east - velocity estimates */
-	float cov_vn_vu;            /**< Covariance between the north - and up - velocity estimates */
-	float cov_vn_dt;            /**< Covariance between the north - velocity and clock drift estimates */
-	float cov_ve_vu;            /**< Covariance between the east - and up - velocity estimates */
-	float cov_ve_dt;            /**< Covariance between the east - velocity and clock drift estimates */
-	float cov_vu_dt;            /**< Covariance between the up - velocity and clock drift estimates */
+    float   cov_vn_vn;           /**< Variance of the north-velocity estimate */
+    float   cov_ve_ve;           /**< Variance of the east-velocity estimate */
+    float   cov_vu_vu;           /**< Variance of the up - velocity estimate */
+    float   cov_dt_dt;           /**< Variance of the clock drift estimate */
+    float   cov_vn_ve;           /**< Covariance between the north - and east - velocity estimates */
+    float   cov_vn_vu;           /**< Covariance between the north - and up - velocity estimates */
+    float   cov_vn_dt;           /**< Covariance between the north - velocity and clock drift estimates */
+    float   cov_ve_vu;           /**< Covariance between the east - and up - velocity estimates */
+    float   cov_ve_dt;           /**< Covariance between the east - velocity and clock drift estimates */
+    float   cov_vu_dt;           /**< Covariance between the up - velocity and clock drift estimates */
 } sbf_payload_vel_cov_geodetic_t;
 
 typedef struct {
-	uint8_t nr_sv;              /**< Total number of satellites used in the PVT computation. */
-	uint8_t reserved;
-	uint16_t pDOP;
-	uint16_t tDOP;
-	uint16_t hDOP;
-	uint16_t vDOP;
-	float hpl;                  /**< Horizontal Protection Level (see the DO229 standard). */
-	float vpl;                  /**< Vertical Protection Level (see the DO229 standard). */
+    uint8_t  nr_sv; /**< Total number of satellites used in the PVT computation. */
+    uint8_t  reserved;
+    uint16_t pDOP;
+    uint16_t tDOP;
+    uint16_t hDOP;
+    uint16_t vDOP;
+    float    hpl; /**< Horizontal Protection Level (see the DO229 standard). */
+    float    vpl; /**< Vertical Protection Level (see the DO229 standard). */
 } sbf_payload_dop_t;
 
 typedef struct {
-	uint8_t antenna;
-	uint8_t reserved;
-	uint16_t tracking_status;
-	uint16_t pvt_status;
-	uint16_t pvt_info;
+    uint8_t  antenna;
+    uint8_t  reserved;
+    uint16_t tracking_status;
+    uint16_t pvt_status;
+    uint16_t pvt_info;
 } sbf_payload_channel_state_info_t;
 
 typedef struct {
-	uint8_t nr_sv;                  /**< The average over all antennas of the number of satellites currently included in the attitude calculations. */
-	uint8_t error_aux1: 2;          /**< Bits 0-1: Error code for Main-Aux1 baseline:
+    uint8_t nr_sv;                   /**< The average over all antennas of the number of satellites currently included in the attitude calculations. */
+    uint8_t error_aux1          : 2; /**< Bits 0-1: Error code for Main-Aux1 baseline:
                                             0: No error
                                             1: Not enough measurements
                                             2: Reserved
                                             3: Reserved */
-	uint8_t error_aux2: 2;          /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */
-	uint8_t error_reserved: 3;      /**< Bits 4-6: Reserved */
-uint8_t error_not_requested:
-	1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */
+    uint8_t error_aux2          : 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */
+    uint8_t error_reserved      : 3; /**< Bits 4-6: Reserved */
+    uint8_t error_not_requested : 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */
 
-	uint16_t mode;                  /**< Attitude mode code:
+    uint16_t mode;                   /**< Attitude mode code:
                                             0: No attitude
                                             1: Heading, pitch (roll = 0), aux antenna positions obtained with float
                                             ambiguities
@@ -279,163 +252,159 @@ uint8_t error_not_requested:
                                             ambiguities
                                             3: Heading, pitch, roll, aux antenna positions obtained with float ambiguities
                                             4: Heading, pitch, roll, aux antenna positions obtained with fixed ambiguities */
-	uint16_t reserved;              /**< Reserved for future use, to be ignored by decoding software */
-
-	float heading;                  /**< Heading */
-	float pitch;                    /**< Pitch */
-	float roll;                     /**< Roll */
-	float pitch_dot;                /**< Rate of change of the pitch angle */
-	float roll_dot;                 /**< Rate of change of the roll angle */
-	float heading_dot;              /**< Rate of change of the heading angle */
+    uint16_t reserved;               /**< Reserved for future use, to be ignored by decoding software */
+
+    float heading;                   /**< Heading */
+    float pitch;                     /**< Pitch */
+    float roll;                      /**< Roll */
+    float pitch_dot;                 /**< Rate of change of the pitch angle */
+    float roll_dot;                  /**< Rate of change of the roll angle */
+    float heading_dot;               /**< Rate of change of the heading angle */
 } sbf_payload_att_euler;
 
 typedef struct {
-	uint8_t reserved;               /**< Reserved for future use, to be ignored by decoding software */
+    uint8_t reserved;                /**< Reserved for future use, to be ignored by decoding software */
 
-	uint8_t error_aux1: 2;          /**< Bits 0-1: Error code for Main-Aux1 baseline:
+    uint8_t error_aux1          : 2; /**< Bits 0-1: Error code for Main-Aux1 baseline:
                                             0: No error
                                             1: Not enough measurements
                                             2: Reserved
                                             3: Reserved */
-	uint8_t error_aux2: 2;          /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */
-	uint8_t error_reserved: 3;      /**< Bits 4-6: Reserved */
-uint8_t error_not_requested:
-	1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */
-
-	float cov_headhead;             /**< Variance of the heading estimate */
-	float cov_pitchpitch;           /**< Variance of the pitch estimate */
-	float cov_rollroll;             /**< Variance of the roll estimate */
-	float cov_headpitch;            /**< Covariance between Euler angle estimates.
+    uint8_t error_aux2          : 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */
+    uint8_t error_reserved      : 3; /**< Bits 4-6: Reserved */
+    uint8_t error_not_requested : 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */
+
+    float cov_headhead;              /**< Variance of the heading estimate */
+    float cov_pitchpitch;            /**< Variance of the pitch estimate */
+    float cov_rollroll;              /**< Variance of the roll estimate */
+    float cov_headpitch;             /**< Covariance between Euler angle estimates.
                                          Future functionality. The values are currently set to their Do-Not-Use values. */
-	float cov_headroll;             /**< Covariance between Euler angle estimates.
+    float cov_headroll;              /**< Covariance between Euler angle estimates.
                                          Future functionality. The values are currently set to their Do-Not-Use values. */
-	float cov_pitchroll;            /**< Covariance between Euler angle estimates.
+    float cov_pitchroll;             /**< Covariance between Euler angle estimates.
                                          Future functionality. The values are currently set to their Do-Not-Use values. */
 } sbf_payload_att_cov_euler;
 
 /* General message and payload buffer union */
 
 typedef struct {
-	uint16_t sync;              /** The Sync field is a 2-byte array always set to 0x24, 0x40. The first byte of every SBF block has
+    uint16_t sync;              /** The Sync field is a 2-byte array always set to 0x24, 0x40. The first byte of every SBF block has
 									hexadecimal value 24 (decimal 36, ASCII '$'). The second byte of every SBF block has hexadecimal
 									value 40 (decimal 64, ASCII '@'). */
-	uint16_t crc16;                /** The CRC field is the 16-bit CRC of all the bytes in an SBF block from and including the ID field
+    uint16_t crc16;             /** The CRC field is the 16-bit CRC of all the bytes in an SBF block from and including the ID field
 									to the last byte of the block. The generator polynomial for this CRC is the so-called CRC-CCITT
 									polynomial: x 16 + x 12 + x 5 + x 0 . The CRC is computed in the forward direction using a seed of 0, no
 									reverse and no final XOR. */
-uint16_t msg_id:
-	13;        /** The ID field is a 2-byte block ID, which uniquely identifies the block type and its contents */
-uint8_t msg_revision:
-	3;    /** block revision number, starting from 0 at the initial block definition, and incrementing
+    uint16_t msg_id       : 13; /** The ID field is a 2-byte block ID, which uniquely identifies the block type and its contents */
+    uint8_t  msg_revision : 3;  /** block revision number, starting from 0 at the initial block definition, and incrementing
                                     each time backwards - compatible changes are performed to the block  */
-	uint16_t length;            /** The Length field is a 2-byte unsigned integer containing the size of the SBF block.
+    uint16_t length;            /** The Length field is a 2-byte unsigned integer containing the size of the SBF block.
                                     It is the total number of bytes in the SBF block including the header.
                                     It is always a multiple of 4. */
-	uint32_t TOW;               /**< Time-Of-Week: Time-tag, expressed in whole milliseconds from
+    uint32_t TOW;               /**< Time-Of-Week: Time-tag, expressed in whole milliseconds from
                                      the beginning of the current Galileo/GPSweek. */
-	uint16_t WNc;               /**< The GPS week number associated with the TOW. WNc is a continuous
+    uint16_t WNc;               /**< The GPS week number associated with the TOW. WNc is a continuous
                                      weekcount (hence the "c"). It is not affected by GPS week roll overs,
                                      which occur every 1024 weeks. By definition of the Galileo system time,
                                      WNc is also the Galileo week number + 1024. */
-	union {
-		sbf_payload_pvt_geodetic_t payload_pvt_geodetic;
-		sbf_payload_vel_cov_geodetic_t payload_vel_col_geodetic;
-		sbf_payload_dop_t payload_dop;
-		sbf_payload_att_euler payload_att_euler;
-		sbf_payload_att_cov_euler payload_att_cov_euler;
-	};
-
-	uint8_t padding[16];
+
+    union {
+        sbf_payload_pvt_geodetic_t     payload_pvt_geodetic;
+        sbf_payload_vel_cov_geodetic_t payload_vel_col_geodetic;
+        sbf_payload_dop_t              payload_dop;
+        sbf_payload_att_euler          payload_att_euler;
+        sbf_payload_att_cov_euler      payload_att_cov_euler;
+    };
+
+    uint8_t padding[16];
 } sbf_buf_t;
 
 #pragma pack(pop)
+
 /*** END OF SBF protocol binary message and payload definitions ***/
 
 /* Decoder state */
 typedef enum {
-	SBF_DECODE_SYNC1 = 0,
-	SBF_DECODE_SYNC2,
-	SBF_DECODE_PAYLOAD,
-	SBF_DECODE_RTCM3
+    SBF_DECODE_SYNC1 = 0,
+    SBF_DECODE_SYNC2,
+    SBF_DECODE_PAYLOAD,
+    SBF_DECODE_RTCM3
 } sbf_decode_state_t;
 
-class GPSDriverSBF : public GPSBaseStationSupport
-{
+class GPSDriverSBF : public GPSBaseStationSupport {
 public:
-	/**
+    /**
 	 * @param heading_offset heading offset in radians [-pi, pi]. It is subtracted from the measurement.
 	 * @param pitch_offset pitch_offset in deg [-90, 90]. This will be send as a cmd to the receiver.
 	 */
-	GPSDriverSBF(GPSCallbackPtr callback,
-		     void *callback_user,
-		     struct sensor_gps_s *gps_position,
-		     satellite_info_s *satellite_info = nullptr,
-		     float heading_offset = 0.f,
-		     float pitch_offset = 0.f);
+    GPSDriverSBF(GPSCallbackPtr       callback,
+                 void                *callback_user,
+                 struct sensor_gps_s *gps_position,
+                 satellite_info_s    *satellite_info = nullptr,
+                 float                heading_offset = 0.f,
+                 float                pitch_offset   = 0.f);
 
-	virtual ~GPSDriverSBF();
+    virtual ~GPSDriverSBF();
 
-	int receive(unsigned timeout) override;
+    int receive(unsigned timeout) override;
 
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
 
-	int reset(GPSRestartType restart_type) override;
+    int reset(GPSRestartType restart_type) override;
 
 private:
-
-	/**
+    /**
 	 * @brief Parse the binary SBF packet
 	 */
-	int parseChar(const uint8_t b);
+    int parseChar(const uint8_t b);
 
-	/**
+    /**
 	 * @brief Add payload rx byte
 	 */
-	int payloadRxAdd(const uint8_t b);
+    int payloadRxAdd(const uint8_t b);
 
-	/**
+    /**
 	 * @brief Parses incoming SBF blocks
 	 */
-	int payloadRxDone(void);
+    int payloadRxDone(void);
 
-	/**
+    /**
 	 * @brief Reset the parse state machine for a fresh start
 	 */
-	void decodeInit(void);
+    void decodeInit(void);
 
-	/**
+    /**
 	 * @brief Send a message
 	 * @return true on success, false on write error (errno set)
 	 */
-	bool sendMessage(const char *msg);
+    bool sendMessage(const char *msg);
 
-	/**
+    /**
 	 * @brief Send a message and waits for acknowledge
 	 * @return true on success, false on write error (errno set) or ack wait timeout
 	 */
-	bool sendMessageAndWaitForAck(const char *msg, const int timeout);
+    bool sendMessageAndWaitForAck(const char *msg, const int timeout);
 
-	/**
+    /**
 	 * @brief Configures the SBF Output blocks
 	 * @return true on success, false on write error (errno set) or ack wait timeout
 	 */
-	bool configSBFOutput(const char *com_port);
-
-	sensor_gps_s *_gps_position{nullptr};
-	satellite_info_s *_satellite_info{nullptr};
-	uint8_t _dynamic_model{7};
-	uint64_t _last_timestamp_time{0};
-	bool _configured{false};
-	uint8_t _msg_status{0};
-	sbf_decode_state_t _decode_state{SBF_DECODE_SYNC1};
-	uint16_t _rx_payload_index{0};
-	sbf_buf_t _buf;
-	OutputMode _output_mode{OutputMode::GPS};
-	RTCMParsing *_rtcm_parsing{nullptr};
-
-	const float _heading_offset;
-	const float _pitch_offset;
+    bool configSBFOutput(const char *com_port);
+
+    sensor_gps_s      *_gps_position{nullptr};
+    satellite_info_s  *_satellite_info{nullptr};
+    uint8_t            _dynamic_model{7};
+    uint64_t           _last_timestamp_time{0};
+    bool               _configured{false};
+    uint8_t            _msg_status{0};
+    sbf_decode_state_t _decode_state{SBF_DECODE_SYNC1};
+    uint16_t           _rx_payload_index{0};
+    sbf_buf_t          _buf;
+    OutputMode         _output_mode{OutputMode::GPS};
+    RTCMParsing       *_rtcm_parsing{nullptr};
+
+    const float _heading_offset;
+    const float _pitch_offset;
 };
 
 uint16_t crc16(const uint8_t *buf, uint32_t len);
-
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/ubx.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/ubx.cpp
index bc327062b8..6abbd3a4e7 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/ubx.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/ubx.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ubx.cpp
@@ -55,2415 +32,2369 @@
 #include "rtcm.h"
 #include "ubx.h"
 
-#define MIN(X,Y)              ((X) < (Y) ? (X) : (Y))
-#define SWAP16(X)             ((((X) >>  8) & 0x00ff) | (((X) << 8) & 0xff00))
+#define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
+#define SWAP16(X) ((((X) >> 8) & 0x00ff) | (((X) << 8) & 0xff00))
 
 /**** Trace macros, disable for production builds */
-#define UBX_TRACE_PARSER(...) {/*GPS_INFO(__VA_ARGS__);*/}    // decoding progress in parse_char()
-#define UBX_TRACE_RXMSG(...)  {/*GPS_INFO(__VA_ARGS__);*/}    // Rx msgs in payload_rx_done()
-#define UBX_TRACE_SVINFO(...) {/*GPS_INFO(__VA_ARGS__);*/}    // NAV-SVINFO processing (debug use only, will cause rx buffer overflows)
+#define UBX_TRACE_PARSER(...)    \
+    { /*GPS_INFO(__VA_ARGS__);*/ \
+    } // decoding progress in parse_char()
+#define UBX_TRACE_RXMSG(...)     \
+    { /*GPS_INFO(__VA_ARGS__);*/ \
+    } // Rx msgs in payload_rx_done()
+#define UBX_TRACE_SVINFO(...)    \
+    { /*GPS_INFO(__VA_ARGS__);*/ \
+    } // NAV-SVINFO processing (debug use only, will cause rx buffer overflows)
 
 /**** Warning macros, disable to save memory */
-#define UBX_WARN(...)         {GPS_WARN(__VA_ARGS__);}
-#define UBX_DEBUG(...)        {/*GPS_WARN(__VA_ARGS__);*/}
+#define UBX_WARN(...) \
+    { GPS_WARN(__VA_ARGS__); }
+#define UBX_DEBUG(...)           \
+    { /*GPS_WARN(__VA_ARGS__);*/ \
+    }
 
 GPSDriverUBX::GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,
-			   sensor_gps_s *gps_position, satellite_info_s *satellite_info, uint8_t dynamic_model,
-			   float heading_offset, int32_t uart2_baudrate, UBXMode mode) :
-	GPSBaseStationSupport(callback, callback_user),
-	_interface(gpsInterface),
-	_gps_position(gps_position),
-	_satellite_info(satellite_info),
-	_dyn_model(dynamic_model),
-	_mode(mode),
-	_heading_offset(heading_offset),
-	_uart2_baudrate(uart2_baudrate)
-{
-	decodeInit();
+                           sensor_gps_s *gps_position, satellite_info_s *satellite_info, uint8_t dynamic_model,
+                           float heading_offset, int32_t uart2_baudrate, UBXMode mode) :
+    GPSBaseStationSupport(callback, callback_user),
+    _interface(gpsInterface),
+    _gps_position(gps_position),
+    _satellite_info(satellite_info),
+    _dyn_model(dynamic_model),
+    _mode(mode),
+    _heading_offset(heading_offset),
+    _uart2_baudrate(uart2_baudrate) {
+    decodeInit();
 }
 
-GPSDriverUBX::~GPSDriverUBX()
-{
-	delete _rtcm_parsing;
+GPSDriverUBX::~GPSDriverUBX() {
+    delete _rtcm_parsing;
 }
 
-int
-GPSDriverUBX::configure(unsigned &baudrate, const GPSConfig &config)
-{
-	_configured = false;
-	_output_mode = config.output_mode;
+int GPSDriverUBX::configure(unsigned &baudrate, const GPSConfig &config) {
+    _configured  = false;
+    _output_mode = config.output_mode;
 
-	ubx_payload_tx_cfg_prt_t cfg_prt[2];
+    ubx_payload_tx_cfg_prt_t cfg_prt[2];
 
-	uint16_t out_proto_mask = _output_mode == OutputMode::GPS ?
-				  UBX_TX_CFG_PRT_PROTO_UBX :
-				  (UBX_TX_CFG_PRT_PROTO_UBX | UBX_TX_CFG_PRT_PROTO_RTCM);
+    uint16_t out_proto_mask = _output_mode == OutputMode::GPS ? UBX_TX_CFG_PRT_PROTO_UBX : (UBX_TX_CFG_PRT_PROTO_UBX | UBX_TX_CFG_PRT_PROTO_RTCM);
 
-	uint16_t in_proto_mask = (_output_mode == OutputMode::GPS || _output_mode == OutputMode::GPSAndRTCM) ?
-				 (UBX_TX_CFG_PRT_PROTO_UBX | UBX_TX_CFG_PRT_PROTO_RTCM) :
-				 UBX_TX_CFG_PRT_PROTO_UBX;
+    uint16_t in_proto_mask = (_output_mode == OutputMode::GPS || _output_mode == OutputMode::GPSAndRTCM) ? (UBX_TX_CFG_PRT_PROTO_UBX | UBX_TX_CFG_PRT_PROTO_RTCM) : UBX_TX_CFG_PRT_PROTO_UBX;
 
-	const bool auto_baudrate = baudrate == 0;
+    const bool auto_baudrate = baudrate == 0;
 
-	if (_interface == Interface::UART) {
+    if (_interface == Interface::UART) {
+        /* try different baudrates */
+        const unsigned baudrates[] = {38400, 57600, 9600, 115200, 230400, 460800, 921600};
 
-		/* try different baudrates */
-		const unsigned baudrates[] = {38400, 57600, 9600, 115200, 230400, 460800, 921600};
+        unsigned baud_i;
+        unsigned desired_baudrate = auto_baudrate ? UBX_BAUDRATE_M8_AND_NEWER : baudrate;
 
-		unsigned baud_i;
-		unsigned desired_baudrate = auto_baudrate ? UBX_BAUDRATE_M8_AND_NEWER : baudrate;
+        if ((_mode == UBXMode::RoverWithMovingBaseUART1) || (_mode == UBXMode::MovingBaseUART1)) {
+            desired_baudrate = UART1_BAUDRATE_HEADING;
+        }
 
-		if ((_mode == UBXMode::RoverWithMovingBaseUART1) || (_mode == UBXMode::MovingBaseUART1)) {
-			desired_baudrate = UART1_BAUDRATE_HEADING;
-		}
+        for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
+            unsigned test_baudrate = baudrates[baud_i];
 
-		for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
-			unsigned test_baudrate = baudrates[baud_i];
+            if (!auto_baudrate && baudrate != test_baudrate) {
+                continue; // skip to next baudrate
+            }
 
-			if (!auto_baudrate && baudrate != test_baudrate) {
-				continue; // skip to next baudrate
-			}
+            UBX_DEBUG("baudrate set to %i", test_baudrate);
 
-			UBX_DEBUG("baudrate set to %i", test_baudrate);
+            setBaudrate(test_baudrate);
 
-			setBaudrate(test_baudrate);
+            /* flush input and wait for at least 20 ms silence */
+            decodeInit();
+            receive(20);
+            decodeInit();
 
-			/* flush input and wait for at least 20 ms silence */
-			decodeInit();
-			receive(20);
-			decodeInit();
+            // try CFG-VALSET: if we get an ACK we know we can use protocol version 27+
+            int cfg_valset_msg_size = initCfgValset();
+            // UART1
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_STOPBITS, 1, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_DATABITS, 0, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_PARITY, 0, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_NMEA, 0, cfg_valset_msg_size);
+            // TODO: are we ever connected to UART2?
 
-			// try CFG-VALSET: if we get an ACK we know we can use protocol version 27+
-			int cfg_valset_msg_size = initCfgValset();
-			// UART1
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_STOPBITS, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_DATABITS, 0, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_PARITY, 0, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_NMEA, 0, cfg_valset_msg_size);
-			// TODO: are we ever connected to UART2?
+            // Note: USB protocol settings are handled later in the configureDevice function.
 
-			// Note: USB protocol settings are handled later in the configureDevice function.
+            bool cfg_valset_success = false;
 
-			bool cfg_valset_success = false;
+            if (sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+                // Note: The M10 comes up sending NMEA sentences at 9600. It can't
+                // respond with an ACK until the current sentence has completed transmission.
+                // This can take over a second so need a large timeout on this particular wait.
+                // Once it has acked this it will turn off the NMEA sentences and all is good
+                // for future transactions.
+                if (waitForAck(UBX_MSG_CFG_VALSET, 2000, true) == 0) {
+                    cfg_valset_success = true;
+                }
+            }
 
-			if (sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            if (cfg_valset_success) {
+                _proto_ver_27_or_higher = true;
+                // Now we only have to change the baudrate
+                cfg_valset_msg_size = initCfgValset();
+                cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART1_BAUDRATE, desired_baudrate, cfg_valset_msg_size);
 
-				// Note: The M10 comes up sending NMEA sentences at 9600. It can't
-				// respond with an ACK until the current sentence has completed transmission.
-				// This can take over a second so need a large timeout on this particular wait.
-				// Once it has acked this it will turn off the NMEA sentences and all is good
-				// for future transactions.
-				if (waitForAck(UBX_MSG_CFG_VALSET, 2000, true) == 0) {
-					cfg_valset_success = true;
-				}
-			}
+                if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+                    continue;
+                }
 
-			if (cfg_valset_success) {
-				_proto_ver_27_or_higher = true;
-				// Now we only have to change the baudrate
-				cfg_valset_msg_size = initCfgValset();
-				cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART1_BAUDRATE, desired_baudrate, cfg_valset_msg_size);
+                /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+                waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
 
-				if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-					continue;
-				}
+            } else {
+                _proto_ver_27_or_higher = false;
 
-				/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-				waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
+                UBX_DEBUG("trying old protocol");
 
-			} else {
-				_proto_ver_27_or_higher = false;
-
-				UBX_DEBUG("trying old protocol");
-
-				/* Send a CFG-PRT message to set the UBX protocol for in and out
+                /* Send a CFG-PRT message to set the UBX protocol for in and out
 				 * and leave the baudrate as it is, we just want an ACK-ACK for this */
-				memset(cfg_prt, 0, 2 * sizeof(ubx_payload_tx_cfg_prt_t));
-				cfg_prt[0].portID		= UBX_TX_CFG_PRT_PORTID;
-				cfg_prt[0].mode		= UBX_TX_CFG_PRT_MODE;
-				cfg_prt[0].baudRate	= test_baudrate;
-				cfg_prt[0].inProtoMask	= in_proto_mask;
-				cfg_prt[0].outProtoMask	= out_proto_mask;
-				cfg_prt[1].portID		= UBX_TX_CFG_PRT_PORTID_USB;
-				cfg_prt[1].mode		= UBX_TX_CFG_PRT_MODE;
-				cfg_prt[1].baudRate	= test_baudrate;
-				cfg_prt[1].inProtoMask	= in_proto_mask;
-				cfg_prt[1].outProtoMask	= out_proto_mask;
-
-				if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
-					continue;
-				}
-
-				if (waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
-					/* try next baudrate */
-					continue;
-				}
-
-				if (auto_baudrate) {
-					desired_baudrate = UBX_TX_CFG_PRT_BAUDRATE;
-				}
-
-				/* Send a CFG-PRT message again, this time change the baudrate */
-				cfg_prt[0].baudRate	= desired_baudrate;
-				cfg_prt[1].baudRate	= desired_baudrate;
-
-				if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
-					continue;
-				}
-
-				/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-				waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
-			}
-
-			if (desired_baudrate != test_baudrate) {
-				setBaudrate(desired_baudrate);
-
-				decodeInit();
-				receive(20);
-				decodeInit();
-			}
-
-			/* at this point we have correct baudrate on both ends */
-			baudrate = desired_baudrate;
-			break;
-		}
-
-		if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
-			return -1;	// connection and/or baudrate detection failed
-		}
-
-	} else if (_interface == Interface::SPI) {
-
-		// try CFG-VALSET: if we get an ACK we know we can use protocol version 27+
-		int cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_SPI_ENABLED, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_SPI_MAXFF, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X, _output_mode == OutputMode::GPS ? 0 : 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA, 0, cfg_valset_msg_size);
-
-		bool cfg_valset_success = false;
-
-		if (sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-
-			if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) == 0) {
-				cfg_valset_success = true;
-			}
-		}
-
-		if (cfg_valset_success) {
-			_proto_ver_27_or_higher = true;
-
-		} else {
-			_proto_ver_27_or_higher = false;
-			memset(cfg_prt, 0, sizeof(ubx_payload_tx_cfg_prt_t));
-			cfg_prt[0].portID		= UBX_TX_CFG_PRT_PORTID_SPI;
-			cfg_prt[0].mode			= UBX_TX_CFG_PRT_MODE_SPI;
-			cfg_prt[0].inProtoMask	= in_proto_mask;
-			cfg_prt[0].outProtoMask	= out_proto_mask;
-
-			if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, sizeof(ubx_payload_tx_cfg_prt_t))) {
-				return -1;
-			}
-
-			waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
-		}
-
-	} else {
-		return -1;
-	}
-
-	UBX_DEBUG("Protocol version 27+: %i", static_cast<int>(_proto_ver_27_or_higher));
-
-	/* Request module version information by sending an empty MON-VER message */
-	if (!sendMessage(UBX_MSG_MON_VER, nullptr, 0)) {
-		return -1;
-	}
-
-	/* Wait for the reply so that we know to which device we're connected (_board will be set).
+                memset(cfg_prt, 0, 2 * sizeof(ubx_payload_tx_cfg_prt_t));
+                cfg_prt[0].portID       = UBX_TX_CFG_PRT_PORTID;
+                cfg_prt[0].mode         = UBX_TX_CFG_PRT_MODE;
+                cfg_prt[0].baudRate     = test_baudrate;
+                cfg_prt[0].inProtoMask  = in_proto_mask;
+                cfg_prt[0].outProtoMask = out_proto_mask;
+                cfg_prt[1].portID       = UBX_TX_CFG_PRT_PORTID_USB;
+                cfg_prt[1].mode         = UBX_TX_CFG_PRT_MODE;
+                cfg_prt[1].baudRate     = test_baudrate;
+                cfg_prt[1].inProtoMask  = in_proto_mask;
+                cfg_prt[1].outProtoMask = out_proto_mask;
+
+                if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
+                    continue;
+                }
+
+                if (waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
+                    /* try next baudrate */
+                    continue;
+                }
+
+                if (auto_baudrate) {
+                    desired_baudrate = UBX_TX_CFG_PRT_BAUDRATE;
+                }
+
+                /* Send a CFG-PRT message again, this time change the baudrate */
+                cfg_prt[0].baudRate = desired_baudrate;
+                cfg_prt[1].baudRate = desired_baudrate;
+
+                if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
+                    continue;
+                }
+
+                /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+                waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
+            }
+
+            if (desired_baudrate != test_baudrate) {
+                setBaudrate(desired_baudrate);
+
+                decodeInit();
+                receive(20);
+                decodeInit();
+            }
+
+            /* at this point we have correct baudrate on both ends */
+            baudrate = desired_baudrate;
+            break;
+        }
+
+        if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
+            return -1; // connection and/or baudrate detection failed
+        }
+
+    } else if (_interface == Interface::SPI) {
+        // try CFG-VALSET: if we get an ACK we know we can use protocol version 27+
+        int cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_SPI_ENABLED, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_SPI_MAXFF, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X, _output_mode == OutputMode::GPS ? 0 : 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA, 0, cfg_valset_msg_size);
+
+        bool cfg_valset_success = false;
+
+        if (sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) == 0) {
+                cfg_valset_success = true;
+            }
+        }
+
+        if (cfg_valset_success) {
+            _proto_ver_27_or_higher = true;
+
+        } else {
+            _proto_ver_27_or_higher = false;
+            memset(cfg_prt, 0, sizeof(ubx_payload_tx_cfg_prt_t));
+            cfg_prt[0].portID       = UBX_TX_CFG_PRT_PORTID_SPI;
+            cfg_prt[0].mode         = UBX_TX_CFG_PRT_MODE_SPI;
+            cfg_prt[0].inProtoMask  = in_proto_mask;
+            cfg_prt[0].outProtoMask = out_proto_mask;
+
+            if (!sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, sizeof(ubx_payload_tx_cfg_prt_t))) {
+                return -1;
+            }
+
+            waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
+        }
+
+    } else {
+        return -1;
+    }
+
+    UBX_DEBUG("Protocol version 27+: %i", static_cast<int>(_proto_ver_27_or_higher));
+
+    /* Request module version information by sending an empty MON-VER message */
+    if (!sendMessage(UBX_MSG_MON_VER, nullptr, 0)) {
+        return -1;
+    }
+
+    /* Wait for the reply so that we know to which device we're connected (_board will be set).
 	 * Note: we won't actually get an ACK-ACK, but UBX_MSG_MON_VER will also set the ack state.
 	 */
-	if (waitForAck(UBX_MSG_MON_VER, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
+    if (waitForAck(UBX_MSG_MON_VER, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
 
 
-	/* Now that we know the board, update the baudrate on M8 boards (on F9+ we already used the
+    /* Now that we know the board, update the baudrate on M8 boards (on F9+ we already used the
 	 * higher baudrate with CFG-VALSET) */
-	if (_interface == Interface::UART && auto_baudrate && _board == Board::u_blox8) {
+    if (_interface == Interface::UART && auto_baudrate && _board == Board::u_blox8) {
+        cfg_prt[0].baudRate = UBX_BAUDRATE_M8_AND_NEWER;
+        cfg_prt[1].baudRate = UBX_BAUDRATE_M8_AND_NEWER;
 
-		cfg_prt[0].baudRate	= UBX_BAUDRATE_M8_AND_NEWER;
-		cfg_prt[1].baudRate	= UBX_BAUDRATE_M8_AND_NEWER;
+        if (sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
+            /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+            waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
 
-		if (sendMessage(UBX_MSG_CFG_PRT, (uint8_t *)cfg_prt, 2 * sizeof(ubx_payload_tx_cfg_prt_t))) {
-			/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-			waitForAck(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
+            setBaudrate(UBX_BAUDRATE_M8_AND_NEWER);
+            baudrate = UBX_BAUDRATE_M8_AND_NEWER;
+        }
+    }
 
-			setBaudrate(UBX_BAUDRATE_M8_AND_NEWER);
-			baudrate = UBX_BAUDRATE_M8_AND_NEWER;
-		}
-	}
 
+    if (_output_mode == OutputMode::RTCM) {
+        // RTCM mode force stationary dynamic model
+        _dyn_model = 2;
+    }
 
-	if (_output_mode == OutputMode::RTCM) {
-		// RTCM mode force stationary dynamic model
-		_dyn_model = 2;
-	}
+    int ret;
 
-	int ret;
+    if (_proto_ver_27_or_higher) {
+        ret = configureDevice(config, _uart2_baudrate);
 
-	if (_proto_ver_27_or_higher) {
-		ret = configureDevice(config, _uart2_baudrate);
+    } else {
+        ret = configureDevicePreV27(config.gnss_systems);
+    }
 
-	} else {
-		ret = configureDevicePreV27(config.gnss_systems);
-	}
+    if (ret != 0) {
+        return ret;
+    }
 
-	if (ret != 0) {
-		return ret;
-	}
+    if (_output_mode == OutputMode::RTCM) {
+        if (restartSurveyIn() < 0) {
+            return -1;
+        }
 
-	if (_output_mode == OutputMode::RTCM) {
-		if (restartSurveyIn() < 0) {
-			return -1;
-		}
+    } else if (_output_mode == OutputMode::GPSAndRTCM) {
+        if (activateRTCMOutput(false) < 0) {
+            return -1;
+        }
+    }
 
-	} else if (_output_mode == OutputMode::GPSAndRTCM) {
-		if (activateRTCMOutput(false) < 0) {
-			return -1;
-		}
-	}
-
-	_configured = true;
-	return 0;
+    _configured = true;
+    return 0;
 }
 
-
-int GPSDriverUBX::configureDevicePreV27(const GNSSSystemsMask &gnssSystems)
-{
-	/* Send a CFG-RATE message to define update rate */
-	memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
-	_buf.payload_tx_cfg_rate.measRate	= UBX_TX_CFG_RATE_MEASINTERVAL;
-	_buf.payload_tx_cfg_rate.navRate	= UBX_TX_CFG_RATE_NAVRATE;
-	_buf.payload_tx_cfg_rate.timeRef	= UBX_TX_CFG_RATE_TIMEREF;
-
-	if (!sendMessage(UBX_MSG_CFG_RATE, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rate))) {
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
-
-	/* send a NAV5 message to set the options for the internal filter */
-	memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
-	_buf.payload_tx_cfg_nav5.mask		= UBX_TX_CFG_NAV5_MASK;
-	_buf.payload_tx_cfg_nav5.dynModel	= _dyn_model;
-	_buf.payload_tx_cfg_nav5.fixMode	= UBX_TX_CFG_NAV5_FIXMODE;
-
-	if (!sendMessage(UBX_MSG_CFG_NAV5, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_nav5))) {
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
-
-	/* configure active GNSS systems (number of channels and used signals taken from U-Center default) */
-	if (static_cast<int32_t>(gnssSystems) != 0) {
-		memset(&_buf.payload_tx_cfg_gnss, 0, sizeof(_buf.payload_tx_cfg_gnss));
-		_buf.payload_tx_cfg_gnss.msgVer = 0x00;
-		_buf.payload_tx_cfg_gnss.numTrkChHw = 0x00;  // read only
-		_buf.payload_tx_cfg_gnss.numTrkChUse = 0xFF;  // use max number of HW channels
-		_buf.payload_tx_cfg_gnss.numConfigBlocks = 7;  // always configure all systems
-
-		// GPS and QZSS should always be enabled and disabled together, according to uBlox
-		_buf.payload_tx_cfg_gnss.block[0].gnssId = UBX_TX_CFG_GNSS_GNSSID_GPS;
-		_buf.payload_tx_cfg_gnss.block[1].gnssId = UBX_TX_CFG_GNSS_GNSSID_QZSS;
-
-		if (gnssSystems & GNSSSystemsMask::ENABLE_GPS) {
-			UBX_DEBUG("GNSS Systems: Use GPS + QZSS");
-			_buf.payload_tx_cfg_gnss.block[0].resTrkCh = 8;
-			_buf.payload_tx_cfg_gnss.block[0].maxTrkCh = 16;
-			_buf.payload_tx_cfg_gnss.block[0].flags = UBX_TX_CFG_GNSS_FLAGS_GPS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-			_buf.payload_tx_cfg_gnss.block[1].resTrkCh = 0;
-			_buf.payload_tx_cfg_gnss.block[1].maxTrkCh = 3;
-			_buf.payload_tx_cfg_gnss.block[1].flags = UBX_TX_CFG_GNSS_FLAGS_QZSS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-		}
-
-		_buf.payload_tx_cfg_gnss.block[2].gnssId = UBX_TX_CFG_GNSS_GNSSID_SBAS;
-
-		if (gnssSystems & GNSSSystemsMask::ENABLE_SBAS) {
-			UBX_DEBUG("GNSS Systems: Use SBAS");
-			_buf.payload_tx_cfg_gnss.block[2].resTrkCh = 1;
-			_buf.payload_tx_cfg_gnss.block[2].maxTrkCh = 3;
-			_buf.payload_tx_cfg_gnss.block[2].flags = UBX_TX_CFG_GNSS_FLAGS_SBAS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-		}
-
-		_buf.payload_tx_cfg_gnss.block[3].gnssId = UBX_TX_CFG_GNSS_GNSSID_GALILEO;
-
-		if (gnssSystems & GNSSSystemsMask::ENABLE_GALILEO) {
-			UBX_DEBUG("GNSS Systems: Use Galileo");
-			_buf.payload_tx_cfg_gnss.block[3].resTrkCh = 4;
-			_buf.payload_tx_cfg_gnss.block[3].maxTrkCh = 8;
-			_buf.payload_tx_cfg_gnss.block[3].flags = UBX_TX_CFG_GNSS_FLAGS_GALILEO_E1 | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-		}
-
-		_buf.payload_tx_cfg_gnss.block[4].gnssId = UBX_TX_CFG_GNSS_GNSSID_BEIDOU;
-
-		if (gnssSystems & GNSSSystemsMask::ENABLE_BEIDOU) {
-			UBX_DEBUG("GNSS Systems: Use BeiDou");
-			_buf.payload_tx_cfg_gnss.block[4].resTrkCh = 8;
-			_buf.payload_tx_cfg_gnss.block[4].maxTrkCh = 16;
-			_buf.payload_tx_cfg_gnss.block[4].flags = UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B1I | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-		}
-
-		_buf.payload_tx_cfg_gnss.block[5].gnssId = UBX_TX_CFG_GNSS_GNSSID_GLONASS;
-
-		if (gnssSystems & GNSSSystemsMask::ENABLE_GLONASS) {
-			UBX_DEBUG("GNSS Systems: Use GLONASS");
-			_buf.payload_tx_cfg_gnss.block[5].resTrkCh = 8;
-			_buf.payload_tx_cfg_gnss.block[5].maxTrkCh = 14;
-			_buf.payload_tx_cfg_gnss.block[5].flags = UBX_TX_CFG_GNSS_FLAGS_GLONASS_L1 | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
-		}
-
-		// IMES always disabled
-		_buf.payload_tx_cfg_gnss.block[6].gnssId = UBX_TX_CFG_GNSS_GNSSID_IMES;
-		_buf.payload_tx_cfg_gnss.block[6].flags = 0;
-
-		// send message
-		if (!sendMessage(UBX_MSG_CFG_GNSS, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_gnss))) {
-			UBX_DEBUG("UBX CFG-GNSS message send failed");
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_GNSS, UBX_CONFIG_TIMEOUT, true) < 0) {
-			UBX_DEBUG("UBX CFG-GNSS message ACK failed");
-			return -1;
-		}
-	}
-
-	/* configure message rates */
-	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
-
-	/* try to set rate for NAV-PVT */
-	/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
-	if (!configureMessageRate(UBX_MSG_NAV_PVT, 1)) {
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
-		_use_nav_pvt = false;
-
-	} else {
-		_use_nav_pvt = true;
-	}
-
-	UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
-
-	if (!_use_nav_pvt) {
-		if (!configureMessageRateAndAck(UBX_MSG_NAV_TIMEUTC, 5, true)) {
-			return -1;
-		}
-
-		if (!configureMessageRateAndAck(UBX_MSG_NAV_POSLLH, 1, true)) {
-			return -1;
-		}
-
-		if (!configureMessageRateAndAck(UBX_MSG_NAV_SOL, 1, true)) {
-			return -1;
-		}
-
-		if (!configureMessageRateAndAck(UBX_MSG_NAV_VELNED, 1, true)) {
-			return -1;
-		}
-	}
-
-	if (!configureMessageRateAndAck(UBX_MSG_NAV_STATUS, 1, true)) {
-		return -1;
-	}
-
-	if (!configureMessageRateAndAck(UBX_MSG_NAV_DOP, 1, true)) {
-		return -1;
-	}
-
-	if (!configureMessageRateAndAck(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0, true)) {
-		return -1;
-	}
-
-	if (!configureMessageRateAndAck(UBX_MSG_MON_HW, 1, true)) {
-		return -1;
-	}
-
-	return 0;
+int GPSDriverUBX::configureDevicePreV27(const GNSSSystemsMask &gnssSystems) {
+    /* Send a CFG-RATE message to define update rate */
+    memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
+    _buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL;
+    _buf.payload_tx_cfg_rate.navRate  = UBX_TX_CFG_RATE_NAVRATE;
+    _buf.payload_tx_cfg_rate.timeRef  = UBX_TX_CFG_RATE_TIMEREF;
+
+    if (!sendMessage(UBX_MSG_CFG_RATE, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rate))) {
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
+
+    /* send a NAV5 message to set the options for the internal filter */
+    memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
+    _buf.payload_tx_cfg_nav5.mask     = UBX_TX_CFG_NAV5_MASK;
+    _buf.payload_tx_cfg_nav5.dynModel = _dyn_model;
+    _buf.payload_tx_cfg_nav5.fixMode  = UBX_TX_CFG_NAV5_FIXMODE;
+
+    if (!sendMessage(UBX_MSG_CFG_NAV5, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_nav5))) {
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
+
+    /* configure active GNSS systems (number of channels and used signals taken from U-Center default) */
+    if (static_cast<int32_t>(gnssSystems) != 0) {
+        memset(&_buf.payload_tx_cfg_gnss, 0, sizeof(_buf.payload_tx_cfg_gnss));
+        _buf.payload_tx_cfg_gnss.msgVer          = 0x00;
+        _buf.payload_tx_cfg_gnss.numTrkChHw      = 0x00; // read only
+        _buf.payload_tx_cfg_gnss.numTrkChUse     = 0xFF; // use max number of HW channels
+        _buf.payload_tx_cfg_gnss.numConfigBlocks = 7;    // always configure all systems
+
+        // GPS and QZSS should always be enabled and disabled together, according to uBlox
+        _buf.payload_tx_cfg_gnss.block[0].gnssId = UBX_TX_CFG_GNSS_GNSSID_GPS;
+        _buf.payload_tx_cfg_gnss.block[1].gnssId = UBX_TX_CFG_GNSS_GNSSID_QZSS;
+
+        if (gnssSystems & GNSSSystemsMask::ENABLE_GPS) {
+            UBX_DEBUG("GNSS Systems: Use GPS + QZSS");
+            _buf.payload_tx_cfg_gnss.block[0].resTrkCh = 8;
+            _buf.payload_tx_cfg_gnss.block[0].maxTrkCh = 16;
+            _buf.payload_tx_cfg_gnss.block[0].flags    = UBX_TX_CFG_GNSS_FLAGS_GPS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+            _buf.payload_tx_cfg_gnss.block[1].resTrkCh = 0;
+            _buf.payload_tx_cfg_gnss.block[1].maxTrkCh = 3;
+            _buf.payload_tx_cfg_gnss.block[1].flags    = UBX_TX_CFG_GNSS_FLAGS_QZSS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+        }
+
+        _buf.payload_tx_cfg_gnss.block[2].gnssId = UBX_TX_CFG_GNSS_GNSSID_SBAS;
+
+        if (gnssSystems & GNSSSystemsMask::ENABLE_SBAS) {
+            UBX_DEBUG("GNSS Systems: Use SBAS");
+            _buf.payload_tx_cfg_gnss.block[2].resTrkCh = 1;
+            _buf.payload_tx_cfg_gnss.block[2].maxTrkCh = 3;
+            _buf.payload_tx_cfg_gnss.block[2].flags    = UBX_TX_CFG_GNSS_FLAGS_SBAS_L1CA | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+        }
+
+        _buf.payload_tx_cfg_gnss.block[3].gnssId = UBX_TX_CFG_GNSS_GNSSID_GALILEO;
+
+        if (gnssSystems & GNSSSystemsMask::ENABLE_GALILEO) {
+            UBX_DEBUG("GNSS Systems: Use Galileo");
+            _buf.payload_tx_cfg_gnss.block[3].resTrkCh = 4;
+            _buf.payload_tx_cfg_gnss.block[3].maxTrkCh = 8;
+            _buf.payload_tx_cfg_gnss.block[3].flags    = UBX_TX_CFG_GNSS_FLAGS_GALILEO_E1 | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+        }
+
+        _buf.payload_tx_cfg_gnss.block[4].gnssId = UBX_TX_CFG_GNSS_GNSSID_BEIDOU;
+
+        if (gnssSystems & GNSSSystemsMask::ENABLE_BEIDOU) {
+            UBX_DEBUG("GNSS Systems: Use BeiDou");
+            _buf.payload_tx_cfg_gnss.block[4].resTrkCh = 8;
+            _buf.payload_tx_cfg_gnss.block[4].maxTrkCh = 16;
+            _buf.payload_tx_cfg_gnss.block[4].flags    = UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B1I | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+        }
+
+        _buf.payload_tx_cfg_gnss.block[5].gnssId = UBX_TX_CFG_GNSS_GNSSID_GLONASS;
+
+        if (gnssSystems & GNSSSystemsMask::ENABLE_GLONASS) {
+            UBX_DEBUG("GNSS Systems: Use GLONASS");
+            _buf.payload_tx_cfg_gnss.block[5].resTrkCh = 8;
+            _buf.payload_tx_cfg_gnss.block[5].maxTrkCh = 14;
+            _buf.payload_tx_cfg_gnss.block[5].flags    = UBX_TX_CFG_GNSS_FLAGS_GLONASS_L1 | UBX_TX_CFG_GNSS_FLAGS_ENABLE;
+        }
+
+        // IMES always disabled
+        _buf.payload_tx_cfg_gnss.block[6].gnssId = UBX_TX_CFG_GNSS_GNSSID_IMES;
+        _buf.payload_tx_cfg_gnss.block[6].flags  = 0;
+
+        // send message
+        if (!sendMessage(UBX_MSG_CFG_GNSS, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_gnss))) {
+            UBX_DEBUG("UBX CFG-GNSS message send failed");
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_GNSS, UBX_CONFIG_TIMEOUT, true) < 0) {
+            UBX_DEBUG("UBX CFG-GNSS message ACK failed");
+            return -1;
+        }
+    }
+
+    /* configure message rates */
+    /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
+
+    /* try to set rate for NAV-PVT */
+    /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
+    if (!configureMessageRate(UBX_MSG_NAV_PVT, 1)) {
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+        _use_nav_pvt = false;
+
+    } else {
+        _use_nav_pvt = true;
+    }
+
+    UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
+
+    if (!_use_nav_pvt) {
+        if (!configureMessageRateAndAck(UBX_MSG_NAV_TIMEUTC, 5, true)) {
+            return -1;
+        }
+
+        if (!configureMessageRateAndAck(UBX_MSG_NAV_POSLLH, 1, true)) {
+            return -1;
+        }
+
+        if (!configureMessageRateAndAck(UBX_MSG_NAV_SOL, 1, true)) {
+            return -1;
+        }
+
+        if (!configureMessageRateAndAck(UBX_MSG_NAV_VELNED, 1, true)) {
+            return -1;
+        }
+    }
+
+    if (!configureMessageRateAndAck(UBX_MSG_NAV_STATUS, 1, true)) {
+        return -1;
+    }
+
+    if (!configureMessageRateAndAck(UBX_MSG_NAV_DOP, 1, true)) {
+        return -1;
+    }
+
+    if (!configureMessageRateAndAck(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0, true)) {
+        return -1;
+    }
+
+    if (!configureMessageRateAndAck(UBX_MSG_MON_HW, 1, true)) {
+        return -1;
+    }
+
+    return 0;
 }
 
-int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_baudrate)
-{
-	// There is no RTCM or USB interface on M10
-	if (_board != Board::u_blox10) {
-
-		int cfg_valset_msg_size = initCfgValset();
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1,
-				   cfg_valset_msg_size);
-
-		if (_output_mode != OutputMode::GPS) {
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
-		}
-
-		// USB
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1,
-				   cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_UBX, 1, cfg_valset_msg_size);
-
-		if (_output_mode != OutputMode::GPS) {
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X, 1, cfg_valset_msg_size);
-		}
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_NMEA, 0, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-	}
-
-	/* set configuration parameters */
-	int cfg_valset_msg_size = initCfgValset();
-	cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_FIXMODE, 3 /* Auto 2d/3d */, cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_UTCSTANDARD, 3 /* USNO (U.S. Naval Observatory derived from GPS) */,
-			   cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_DYNMODEL, _dyn_model, cfg_valset_msg_size);
-
-	// disable odometer & filtering
-	cfgValset<uint8_t>(UBX_CFG_KEY_ODO_USE_ODO, 0, cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_ODO_USE_COG, 0, cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_ODO_OUTLPVEL, 0, cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_ODO_OUTLPCOG, 0, cfg_valset_msg_size);
-
-	// enable jamming monitor
-	cfgValset<uint8_t>(UBX_CFG_KEY_ITFM_ENABLE, 1, cfg_valset_msg_size);
-
-	// measurement rate
-	// In case of F9P not in moving base mode we use 10Hz, otherwise 8Hz (receivers such as M9N can go higher as well, but
-	// the number of used satellites will be restricted to 16. Not mentioned in datasheet)
-	int rate_meas;
-
-	if (_mode != UBXMode::Normal) {
-		rate_meas = 125; //8Hz for heading.
-
-	} else {
-		rate_meas = (_board == Board::u_blox9_F9P) ? 100 : 125;
-	}
-
-	cfgValset<uint16_t>(UBX_CFG_KEY_RATE_MEAS, rate_meas, cfg_valset_msg_size);
-	cfgValset<uint16_t>(UBX_CFG_KEY_RATE_NAV, 1, cfg_valset_msg_size);
-	cfgValset<uint8_t>(UBX_CFG_KEY_RATE_TIMEREF, 0, cfg_valset_msg_size);
-
-	if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
-
-	// RTK (optional, as only RTK devices like F9P support it)
-	cfg_valset_msg_size = initCfgValset();
-	cfgValset<uint8_t>(UBX_CFG_KEY_NAVHPG_DGNSSMODE, 3 /* RTK Fixed */, cfg_valset_msg_size);
-
-	if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-		return -1;
-	}
-
-	waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
-
-	// configure active GNSS systems (leave signal bands as is)
-	// Note: For M10 configuration if changing from default. As per the
-	//       MAX-M10S integration guide UBX-20053088 - R03, see section
-	//       2.1.1.3 GNSS signal configuration for details on some restrictions.
-	//       Implementing these restrictions are a TODO item for M10.
-	if (static_cast<int32_t>(config.gnss_systems) != 0) {
-		cfg_valset_msg_size = initCfgValset();
-
-		// GPS and QZSS should always be enabled and disabled together, according to uBlox
-		if (config.gnss_systems & GNSSSystemsMask::ENABLE_GPS) {
-			UBX_DEBUG("GNSS Systems: Use GPS + QZSS");
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GPS_ENA, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_QZSS_ENA, 1, cfg_valset_msg_size);
-
-		} else {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GPS_ENA, 0, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_QZSS_ENA, 0, cfg_valset_msg_size);
-		}
-
-		if (config.gnss_systems & GNSSSystemsMask::ENABLE_GALILEO) {
-			UBX_DEBUG("GNSS Systems: Use Galileo");
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GAL_ENA, 1, cfg_valset_msg_size);
-
-		} else {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GAL_ENA, 0, cfg_valset_msg_size);
-		}
-
-		if (config.gnss_systems & GNSSSystemsMask::ENABLE_BEIDOU) {
-			UBX_DEBUG("GNSS Systems: Use BeiDou");
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_BDS_ENA, 1, cfg_valset_msg_size);
-
-		} else {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_BDS_ENA, 0, cfg_valset_msg_size);
-		}
-
-		if (config.gnss_systems & GNSSSystemsMask::ENABLE_GLONASS) {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GLO_ENA, 1, cfg_valset_msg_size);
-
-		} else {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GLO_ENA, 0, cfg_valset_msg_size);
-		}
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			UBX_DEBUG("UBX GNSS config send failed");
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-		// send SBAS config separately, because it seems to be buggy (with u-center, too)
-		cfg_valset_msg_size = initCfgValset();
-
-		if (config.gnss_systems & GNSSSystemsMask::ENABLE_SBAS) {
-			UBX_DEBUG("GNSS Systems: Use SBAS");
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_ENA, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_L1CA_ENA, 1, cfg_valset_msg_size);
-
-		} else {
-			cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_ENA, 0, cfg_valset_msg_size);
-		}
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true);
-	}
-
-	// Configure message rates
-	// Send a new CFG-VALSET message to make sure it does not get too large
-	cfg_valset_msg_size = initCfgValset();
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C, 1, cfg_valset_msg_size);
-	_use_nav_pvt = true;
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C, 1, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C, (_satellite_info != nullptr) ? 10 : 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_STATUS_I2C, 1, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C, 1, cfg_valset_msg_size);
-
-	if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
-
-	if (_interface == Interface::UART || _interface == Interface::SPI) {
-
-		// Enable/Disable GPS protocols at I2C interface
-		cfg_valset_msg_size = initCfgValset();
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_UBX,
-				   config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_UBX, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_NMEA,
-				   config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_NMEA, cfg_valset_msg_size);
-
-		// There is no RTCM on M10
-		if (_board != Board::u_blox10) {
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_RTCM3X,
-					   config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_RTCM3X, cfg_valset_msg_size);
-		}
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_UBX,
-				   config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_UBX, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_NMEA,
-				   config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_NMEA, cfg_valset_msg_size);
-
-		if (_board == Board::u_blox9_F9P) {
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_RTCM3X,
-					   config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_RTCM3X, cfg_valset_msg_size);
-		}
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-	}
-
-	if (_mode == UBXMode::RoverWithStaticBaseUart2 || _mode == UBXMode::RoverWithMovingBase) {
-		UBX_DEBUG("Configuring UART2 for rover");
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
-		// heading output period 1 second
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
-		// enable RTCM input on uart2 + set baudrate
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_STOPBITS, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_DATABITS, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_PARITY, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_UBX, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_UBX, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
-		cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART2_BAUDRATE, uart2_baudrate, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-	} else if (_mode == UBXMode::MovingBase) {
-		UBX_DEBUG("Configuring UART2 for moving base");
-		// enable RTCM output on uart2 + set baudrate
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_STOPBITS, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_DATABITS, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_PARITY, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_UBX, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_UBX, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
-		cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART2_BAUDRATE, uart2_baudrate, cfg_valset_msg_size);
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART2, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART2, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART2, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART2, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART2, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART2, 1, cfg_valset_msg_size);
-
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-	} else if (_mode == UBXMode::RoverWithMovingBaseUART1) {
-		UBX_DEBUG("Configuring UART1 for rover");
-		// heading output period 1 second
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-	} else if (_mode == UBXMode::MovingBaseUART1) {
-		UBX_DEBUG("Configuring UART1 for moving base");
-		// enable RTCM output on uart1
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 0, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2, 0, cfg_valset_msg_size);
-
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1, 1, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-	}
-
-	return 0;
+int GPSDriverUBX::configureDevice(const GPSConfig &config, const int32_t uart2_baudrate) {
+    // There is no RTCM or USB interface on M10
+    if (_board != Board::u_blox10) {
+        int cfg_valset_msg_size = initCfgValset();
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1,
+                           cfg_valset_msg_size);
+
+        if (_output_mode != OutputMode::GPS) {
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
+        }
+
+        // USB
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_RTCM3X, _output_mode == OutputMode::RTCM ? 0 : 1,
+                           cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_UBX, 1, cfg_valset_msg_size);
+
+        if (_output_mode != OutputMode::GPS) {
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X, 1, cfg_valset_msg_size);
+        }
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_NMEA, 0, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+    }
+
+    /* set configuration parameters */
+    int cfg_valset_msg_size = initCfgValset();
+    cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_FIXMODE, 3 /* Auto 2d/3d */, cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_UTCSTANDARD, 3 /* USNO (U.S. Naval Observatory derived from GPS) */,
+                       cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_NAVSPG_DYNMODEL, _dyn_model, cfg_valset_msg_size);
+
+    // disable odometer & filtering
+    cfgValset<uint8_t>(UBX_CFG_KEY_ODO_USE_ODO, 0, cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_ODO_USE_COG, 0, cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_ODO_OUTLPVEL, 0, cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_ODO_OUTLPCOG, 0, cfg_valset_msg_size);
+
+    // enable jamming monitor
+    cfgValset<uint8_t>(UBX_CFG_KEY_ITFM_ENABLE, 1, cfg_valset_msg_size);
+
+    // measurement rate
+    // In case of F9P not in moving base mode we use 10Hz, otherwise 8Hz (receivers such as M9N can go higher as well, but
+    // the number of used satellites will be restricted to 16. Not mentioned in datasheet)
+    int rate_meas;
+
+    if (_mode != UBXMode::Normal) {
+        rate_meas = 125; //8Hz for heading.
+
+    } else {
+        rate_meas = (_board == Board::u_blox9_F9P) ? 100 : 125;
+    }
+
+    cfgValset<uint16_t>(UBX_CFG_KEY_RATE_MEAS, rate_meas, cfg_valset_msg_size);
+    cfgValset<uint16_t>(UBX_CFG_KEY_RATE_NAV, 1, cfg_valset_msg_size);
+    cfgValset<uint8_t>(UBX_CFG_KEY_RATE_TIMEREF, 0, cfg_valset_msg_size);
+
+    if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
+
+    // RTK (optional, as only RTK devices like F9P support it)
+    cfg_valset_msg_size = initCfgValset();
+    cfgValset<uint8_t>(UBX_CFG_KEY_NAVHPG_DGNSSMODE, 3 /* RTK Fixed */, cfg_valset_msg_size);
+
+    if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+        return -1;
+    }
+
+    waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
+
+    // configure active GNSS systems (leave signal bands as is)
+    // Note: For M10 configuration if changing from default. As per the
+    //       MAX-M10S integration guide UBX-20053088 - R03, see section
+    //       2.1.1.3 GNSS signal configuration for details on some restrictions.
+    //       Implementing these restrictions are a TODO item for M10.
+    if (static_cast<int32_t>(config.gnss_systems) != 0) {
+        cfg_valset_msg_size = initCfgValset();
+
+        // GPS and QZSS should always be enabled and disabled together, according to uBlox
+        if (config.gnss_systems & GNSSSystemsMask::ENABLE_GPS) {
+            UBX_DEBUG("GNSS Systems: Use GPS + QZSS");
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GPS_ENA, 1, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_QZSS_ENA, 1, cfg_valset_msg_size);
+
+        } else {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GPS_ENA, 0, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_QZSS_ENA, 0, cfg_valset_msg_size);
+        }
+
+        if (config.gnss_systems & GNSSSystemsMask::ENABLE_GALILEO) {
+            UBX_DEBUG("GNSS Systems: Use Galileo");
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GAL_ENA, 1, cfg_valset_msg_size);
+
+        } else {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GAL_ENA, 0, cfg_valset_msg_size);
+        }
+
+        if (config.gnss_systems & GNSSSystemsMask::ENABLE_BEIDOU) {
+            UBX_DEBUG("GNSS Systems: Use BeiDou");
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_BDS_ENA, 1, cfg_valset_msg_size);
+
+        } else {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_BDS_ENA, 0, cfg_valset_msg_size);
+        }
+
+        if (config.gnss_systems & GNSSSystemsMask::ENABLE_GLONASS) {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GLO_ENA, 1, cfg_valset_msg_size);
+
+        } else {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_GLO_ENA, 0, cfg_valset_msg_size);
+        }
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            UBX_DEBUG("UBX GNSS config send failed");
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+        // send SBAS config separately, because it seems to be buggy (with u-center, too)
+        cfg_valset_msg_size = initCfgValset();
+
+        if (config.gnss_systems & GNSSSystemsMask::ENABLE_SBAS) {
+            UBX_DEBUG("GNSS Systems: Use SBAS");
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_ENA, 1, cfg_valset_msg_size);
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_L1CA_ENA, 1, cfg_valset_msg_size);
+
+        } else {
+            cfgValset<uint8_t>(UBX_CFG_KEY_SIGNAL_SBAS_ENA, 0, cfg_valset_msg_size);
+        }
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true);
+    }
+
+    // Configure message rates
+    // Send a new CFG-VALSET message to make sure it does not get too large
+    cfg_valset_msg_size = initCfgValset();
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C, 1, cfg_valset_msg_size);
+    _use_nav_pvt = true;
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C, 1, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C, (_satellite_info != nullptr) ? 10 : 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_STATUS_I2C, 1, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C, 1, cfg_valset_msg_size);
+
+    if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
+
+    if (_interface == Interface::UART || _interface == Interface::SPI) {
+        // Enable/Disable GPS protocols at I2C interface
+        cfg_valset_msg_size = initCfgValset();
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_UBX,
+                           config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_UBX, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_NMEA,
+                           config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_NMEA, cfg_valset_msg_size);
+
+        // There is no RTCM on M10
+        if (_board != Board::u_blox10) {
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2CINPROT_RTCM3X,
+                               config.interface_protocols & InterfaceProtocolsMask::I2C_IN_PROT_RTCM3X, cfg_valset_msg_size);
+        }
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_UBX,
+                           config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_UBX, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_NMEA,
+                           config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_NMEA, cfg_valset_msg_size);
+
+        if (_board == Board::u_blox9_F9P) {
+            cfgValset<uint8_t>(UBX_CFG_KEY_CFG_I2COUTPROT_RTCM3X,
+                               config.interface_protocols & InterfaceProtocolsMask::I2C_OUT_PROT_RTCM3X, cfg_valset_msg_size);
+        }
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+    }
+
+    if (_mode == UBXMode::RoverWithStaticBaseUart2 || _mode == UBXMode::RoverWithMovingBase) {
+        UBX_DEBUG("Configuring UART2 for rover");
+        cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
+        // heading output period 1 second
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
+        // enable RTCM input on uart2 + set baudrate
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_STOPBITS, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_DATABITS, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_PARITY, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_UBX, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_UBX, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
+        cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART2_BAUDRATE, uart2_baudrate, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+    } else if (_mode == UBXMode::MovingBase) {
+        UBX_DEBUG("Configuring UART2 for moving base");
+        // enable RTCM output on uart2 + set baudrate
+        cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_STOPBITS, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_DATABITS, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2_PARITY, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_UBX, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2INPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_UBX, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
+        cfgValset<uint32_t>(UBX_CFG_KEY_CFG_UART2_BAUDRATE, uart2_baudrate, cfg_valset_msg_size);
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART2, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART2, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART2, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART2, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART2, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART2, 1, cfg_valset_msg_size);
+
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+    } else if (_mode == UBXMode::RoverWithMovingBaseUART1) {
+        UBX_DEBUG("Configuring UART1 for rover");
+        // heading output period 1 second
+        cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 1, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+    } else if (_mode == UBXMode::MovingBaseUART1) {
+        UBX_DEBUG("Configuring UART1 for moving base");
+        // enable RTCM output on uart1
+        cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1, 0, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2, 0, cfg_valset_msg_size);
+
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1, 1, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1, 1, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+    }
+
+    return 0;
 }
 
-int GPSDriverUBX::initCfgValset()
-{
-	memset(&_buf.payload_tx_cfg_valset, 0, sizeof(_buf.payload_tx_cfg_valset));
-	_buf.payload_tx_cfg_valset.layers = UBX_CFG_LAYER_RAM;
-	return sizeof(_buf.payload_tx_cfg_valset) - sizeof(_buf.payload_tx_cfg_valset.cfgData);
+int GPSDriverUBX::initCfgValset() {
+    memset(&_buf.payload_tx_cfg_valset, 0, sizeof(_buf.payload_tx_cfg_valset));
+    _buf.payload_tx_cfg_valset.layers = UBX_CFG_LAYER_RAM;
+    return sizeof(_buf.payload_tx_cfg_valset) - sizeof(_buf.payload_tx_cfg_valset.cfgData);
 }
 
-template<typename T>
-bool GPSDriverUBX::cfgValset(uint32_t key_id, T value, int &msg_size)
-{
-	if (msg_size + sizeof(key_id) + sizeof(value) > sizeof(_buf)) {
-		// If this happens use several CFG-VALSET messages instead of one
-		UBX_WARN("buf for CFG_VALSET too small");
-		return false;
-	}
-
-	uint8_t *buffer = (uint8_t *)&_buf.payload_tx_cfg_valset;
-	memcpy(buffer + msg_size, &key_id, sizeof(key_id));
-	msg_size += sizeof(key_id);
-	memcpy(buffer + msg_size, &value, sizeof(value));
-	msg_size += sizeof(value);
-	return true;
+template <typename T>
+bool GPSDriverUBX::cfgValset(uint32_t key_id, T value, int &msg_size) {
+    if (msg_size + sizeof(key_id) + sizeof(value) > sizeof(_buf)) {
+        // If this happens use several CFG-VALSET messages instead of one
+        UBX_WARN("buf for CFG_VALSET too small");
+        return false;
+    }
+
+    uint8_t *buffer = (uint8_t *)&_buf.payload_tx_cfg_valset;
+    memcpy(buffer + msg_size, &key_id, sizeof(key_id));
+    msg_size += sizeof(key_id);
+    memcpy(buffer + msg_size, &value, sizeof(value));
+    msg_size += sizeof(value);
+    return true;
 }
 
-bool GPSDriverUBX::cfgValsetPort(uint32_t key_id, uint8_t value, int &msg_size)
-{
-	if (_interface == Interface::SPI) {
-		if (!cfgValset<uint8_t>(key_id + 4, value, msg_size)) {
-			return false;
-		}
-
-	} else {
-		// enable on UART1 & USB (TODO: should we enable UART2 too? -> better would be to detect the port)
-		if (!cfgValset<uint8_t>(key_id + 1, value, msg_size)) {
-			return false;
-		}
-
-		// M10 has no USB
-		if (_board != Board::u_blox10) {
-			if (!cfgValset<uint8_t>(key_id + 3, value, msg_size)) {
-				return false;
-			}
-		}
-	}
-
-	return true;
+bool GPSDriverUBX::cfgValsetPort(uint32_t key_id, uint8_t value, int &msg_size) {
+    if (_interface == Interface::SPI) {
+        if (!cfgValset<uint8_t>(key_id + 4, value, msg_size)) {
+            return false;
+        }
+
+    } else {
+        // enable on UART1 & USB (TODO: should we enable UART2 too? -> better would be to detect the port)
+        if (!cfgValset<uint8_t>(key_id + 1, value, msg_size)) {
+            return false;
+        }
+
+        // M10 has no USB
+        if (_board != Board::u_blox10) {
+            if (!cfgValset<uint8_t>(key_id + 3, value, msg_size)) {
+                return false;
+            }
+        }
+    }
+
+    return true;
 }
 
-int GPSDriverUBX::restartSurveyInPreV27()
-{
-	//disable RTCM output
-	configureMessageRate(UBX_MSG_RTCM3_1005, 0);
-	configureMessageRate(UBX_MSG_RTCM3_1077, 0);
-	configureMessageRate(UBX_MSG_RTCM3_1087, 0);
-	configureMessageRate(UBX_MSG_RTCM3_1230, 0);
-	configureMessageRate(UBX_MSG_RTCM3_1097, 0);
-	configureMessageRate(UBX_MSG_RTCM3_1127, 0);
-
-	//stop it first
-	//FIXME: stopping the survey-in process does not seem to work
-	memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
-	_buf.payload_tx_cfg_tmode3.flags        = 0; /* disable time mode */
-
-	if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
-		UBX_WARN("TMODE3 failed. Device w/o base station support?");
-		return -1;
-	}
-
-	if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
-		return -1;
-	}
-
-	if (_base_settings.type == BaseSettingsType::survey_in) {
-		UBX_DEBUG("Starting Survey-in");
-
-		memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
-		_buf.payload_tx_cfg_tmode3.flags        = 1; /* start survey-in */
-		_buf.payload_tx_cfg_tmode3.svinMinDur   = _base_settings.settings.survey_in.min_dur;
-		_buf.payload_tx_cfg_tmode3.svinAccLimit = _base_settings.settings.survey_in.acc_limit;
-
-		if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-		/* enable status output of survey-in */
-		if (!configureMessageRateAndAck(UBX_MSG_NAV_SVIN, 5, true)) {
-			return -1;
-		}
-
-	} else {
-		UBX_DEBUG("Setting fixed base position");
-
-		const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
-
-		memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
-		_buf.payload_tx_cfg_tmode3.flags = 2 /* fixed mode */ | (1 << 8) /* lat/lon mode */;
-		int64_t lat64 = (int64_t)(settings.latitude * 1e9);
-		_buf.payload_tx_cfg_tmode3.ecefXOrLat = (int32_t)(lat64 / 100);
-		_buf.payload_tx_cfg_tmode3.ecefXOrLatHP = lat64 % 100; // range [-99, 99]
-		int64_t lon64 = (int64_t)(settings.longitude * 1e9);
-		_buf.payload_tx_cfg_tmode3.ecefYOrLon = (int32_t)(lon64 / 100);
-		_buf.payload_tx_cfg_tmode3.ecefYOrLonHP = lon64 % 100;
-		int64_t alt64 = (int64_t)((double)settings.altitude * 1e4);
-		_buf.payload_tx_cfg_tmode3.ecefZOrAlt = (int32_t)(alt64 / 100); // cm
-		_buf.payload_tx_cfg_tmode3.ecefZOrAltHP = alt64 % 100; // 0.1mm
-
-		_buf.payload_tx_cfg_tmode3.fixedPosAcc = (uint32_t)(settings.position_accuracy * 10.f);
-
-		if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-		// directly enable RTCM3 output
-		return activateRTCMOutput(true);
-	}
-
-	return 0;
+int GPSDriverUBX::restartSurveyInPreV27() {
+    //disable RTCM output
+    configureMessageRate(UBX_MSG_RTCM3_1005, 0);
+    configureMessageRate(UBX_MSG_RTCM3_1077, 0);
+    configureMessageRate(UBX_MSG_RTCM3_1087, 0);
+    configureMessageRate(UBX_MSG_RTCM3_1230, 0);
+    configureMessageRate(UBX_MSG_RTCM3_1097, 0);
+    configureMessageRate(UBX_MSG_RTCM3_1127, 0);
+
+    //stop it first
+    //FIXME: stopping the survey-in process does not seem to work
+    memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
+    _buf.payload_tx_cfg_tmode3.flags = 0; /* disable time mode */
+
+    if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
+        UBX_WARN("TMODE3 failed. Device w/o base station support?");
+        return -1;
+    }
+
+    if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
+        return -1;
+    }
+
+    if (_base_settings.type == BaseSettingsType::survey_in) {
+        UBX_DEBUG("Starting Survey-in");
+
+        memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
+        _buf.payload_tx_cfg_tmode3.flags        = 1; /* start survey-in */
+        _buf.payload_tx_cfg_tmode3.svinMinDur   = _base_settings.settings.survey_in.min_dur;
+        _buf.payload_tx_cfg_tmode3.svinAccLimit = _base_settings.settings.survey_in.acc_limit;
+
+        if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+        /* enable status output of survey-in */
+        if (!configureMessageRateAndAck(UBX_MSG_NAV_SVIN, 5, true)) {
+            return -1;
+        }
+
+    } else {
+        UBX_DEBUG("Setting fixed base position");
+
+        const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
+
+        memset(&_buf.payload_tx_cfg_tmode3, 0, sizeof(_buf.payload_tx_cfg_tmode3));
+        _buf.payload_tx_cfg_tmode3.flags        = 2 /* fixed mode */ | (1 << 8) /* lat/lon mode */;
+        int64_t lat64                           = (int64_t)(settings.latitude * 1e9);
+        _buf.payload_tx_cfg_tmode3.ecefXOrLat   = (int32_t)(lat64 / 100);
+        _buf.payload_tx_cfg_tmode3.ecefXOrLatHP = lat64 % 100; // range [-99, 99]
+        int64_t lon64                           = (int64_t)(settings.longitude * 1e9);
+        _buf.payload_tx_cfg_tmode3.ecefYOrLon   = (int32_t)(lon64 / 100);
+        _buf.payload_tx_cfg_tmode3.ecefYOrLonHP = lon64 % 100;
+        int64_t alt64                           = (int64_t)((double)settings.altitude * 1e4);
+        _buf.payload_tx_cfg_tmode3.ecefZOrAlt   = (int32_t)(alt64 / 100); // cm
+        _buf.payload_tx_cfg_tmode3.ecefZOrAltHP = alt64 % 100;            // 0.1mm
+
+        _buf.payload_tx_cfg_tmode3.fixedPosAcc = (uint32_t)(settings.position_accuracy * 10.f);
+
+        if (!sendMessage(UBX_MSG_CFG_TMODE3, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_tmode3))) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_TMODE3, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+        // directly enable RTCM3 output
+        return activateRTCMOutput(true);
+    }
+
+    return 0;
 }
 
-int GPSDriverUBX::restartSurveyIn()
-{
-	if (_output_mode != OutputMode::RTCM) {
-		return -1;
-	}
-
-	if (!_proto_ver_27_or_higher) {
-		return restartSurveyInPreV27();
-	}
-
-	//disable RTCM output
-	int cfg_valset_msg_size = initCfgValset();
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, 0, cfg_valset_msg_size);
-	cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, 0, cfg_valset_msg_size);
-	sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size);
-	waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
-
-	if (_base_settings.type == BaseSettingsType::survey_in) {
-		UBX_DEBUG("Starting Survey-in");
-
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_MODE, 1 /* Survey-in */, cfg_valset_msg_size);
-		cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_SVIN_MIN_DUR, _base_settings.settings.survey_in.min_dur, cfg_valset_msg_size);
-		cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT, _base_settings.settings.survey_in.acc_limit, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, 5, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-	} else {
-		UBX_DEBUG("Setting fixed base position");
-
-		const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
-		cfg_valset_msg_size = initCfgValset();
-		cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_MODE, 2 /* Fixed Mode */, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_POS_TYPE, 1 /* Lat/Lon/Height */, cfg_valset_msg_size);
-		int64_t lat64 = (int64_t)(settings.latitude * 1e9);
-		cfgValset<int32_t>(UBX_CFG_KEY_TMODE_LAT, (int32_t)(lat64 / 100), cfg_valset_msg_size);
-		cfgValset<int8_t>(UBX_CFG_KEY_TMODE_LAT_HP, lat64 % 100 /* range [-99, 99] */, cfg_valset_msg_size);
-		int64_t lon64 = (int64_t)(settings.longitude * 1e9);
-		cfgValset<int32_t>(UBX_CFG_KEY_TMODE_LON, (int32_t)(lon64 / 100), cfg_valset_msg_size);
-		cfgValset<int8_t>(UBX_CFG_KEY_TMODE_LON_HP, lon64 % 100 /* range [-99, 99] */, cfg_valset_msg_size);
-		int64_t alt64 = (int64_t)((double)settings.altitude * 1e4);
-		cfgValset<int32_t>(UBX_CFG_KEY_TMODE_HEIGHT, (int32_t)(alt64 / 100) /* cm */, cfg_valset_msg_size);
-		cfgValset<int8_t>(UBX_CFG_KEY_TMODE_HEIGHT_HP, alt64 % 100 /* 0.1mm */, cfg_valset_msg_size);
-		cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_FIXED_POS_ACC, (uint32_t)(settings.position_accuracy * 10.f),
-				    cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
-			return -1;
-		}
-
-		// directly enable RTCM3 output
-		return activateRTCMOutput(true);
-
-	}
-
-	return 0;
+int GPSDriverUBX::restartSurveyIn() {
+    if (_output_mode != OutputMode::RTCM) {
+        return -1;
+    }
+
+    if (!_proto_ver_27_or_higher) {
+        return restartSurveyInPreV27();
+    }
+
+    //disable RTCM output
+    int cfg_valset_msg_size = initCfgValset();
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, 0, cfg_valset_msg_size);
+    cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, 0, cfg_valset_msg_size);
+    sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size);
+    waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false);
+
+    if (_base_settings.type == BaseSettingsType::survey_in) {
+        UBX_DEBUG("Starting Survey-in");
+
+        cfg_valset_msg_size = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_MODE, 1 /* Survey-in */, cfg_valset_msg_size);
+        cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_SVIN_MIN_DUR, _base_settings.settings.survey_in.min_dur, cfg_valset_msg_size);
+        cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT, _base_settings.settings.survey_in.acc_limit, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, 5, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+    } else {
+        UBX_DEBUG("Setting fixed base position");
+
+        const FixedPositionSettings &settings = _base_settings.settings.fixed_position;
+        cfg_valset_msg_size                   = initCfgValset();
+        cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_MODE, 2 /* Fixed Mode */, cfg_valset_msg_size);
+        cfgValset<uint8_t>(UBX_CFG_KEY_TMODE_POS_TYPE, 1 /* Lat/Lon/Height */, cfg_valset_msg_size);
+        int64_t lat64 = (int64_t)(settings.latitude * 1e9);
+        cfgValset<int32_t>(UBX_CFG_KEY_TMODE_LAT, (int32_t)(lat64 / 100), cfg_valset_msg_size);
+        cfgValset<int8_t>(UBX_CFG_KEY_TMODE_LAT_HP, lat64 % 100 /* range [-99, 99] */, cfg_valset_msg_size);
+        int64_t lon64 = (int64_t)(settings.longitude * 1e9);
+        cfgValset<int32_t>(UBX_CFG_KEY_TMODE_LON, (int32_t)(lon64 / 100), cfg_valset_msg_size);
+        cfgValset<int8_t>(UBX_CFG_KEY_TMODE_LON_HP, lon64 % 100 /* range [-99, 99] */, cfg_valset_msg_size);
+        int64_t alt64 = (int64_t)((double)settings.altitude * 1e4);
+        cfgValset<int32_t>(UBX_CFG_KEY_TMODE_HEIGHT, (int32_t)(alt64 / 100) /* cm */, cfg_valset_msg_size);
+        cfgValset<int8_t>(UBX_CFG_KEY_TMODE_HEIGHT_HP, alt64 % 100 /* 0.1mm */, cfg_valset_msg_size);
+        cfgValset<uint32_t>(UBX_CFG_KEY_TMODE_FIXED_POS_ACC, (uint32_t)(settings.position_accuracy * 10.f),
+                            cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
+            return -1;
+        }
+
+        // directly enable RTCM3 output
+        return activateRTCMOutput(true);
+    }
+
+    return 0;
 }
 
-int	// -1 = NAK, error or timeout, 0 = ACK
-GPSDriverUBX::waitForAck(const uint16_t msg, const unsigned timeout, const bool report)
-{
-	int ret = -1;
+int // -1 = NAK, error or timeout, 0 = ACK
+GPSDriverUBX::waitForAck(const uint16_t msg, const unsigned timeout, const bool report) {
+    int ret = -1;
 
-	_ack_state = UBX_ACK_WAITING;
-	_ack_waiting_msg = msg;	// memorize sent msg class&ID for ACK check
+    _ack_state       = UBX_ACK_WAITING;
+    _ack_waiting_msg = msg; // memorize sent msg class&ID for ACK check
 
-	gps_abstime time_started = gps_absolute_time();
+    gps_abstime time_started = gps_absolute_time();
 
-	while ((_ack_state == UBX_ACK_WAITING) && (gps_absolute_time() < time_started + timeout * 1000)) {
-		receive(timeout);
-	}
+    while ((_ack_state == UBX_ACK_WAITING) && (gps_absolute_time() < time_started + timeout * 1000)) {
+        receive(timeout);
+    }
 
-	if (_ack_state == UBX_ACK_GOT_ACK) {
-		ret = 0;	// ACK received ok
+    if (_ack_state == UBX_ACK_GOT_ACK) {
+        ret = 0; // ACK received ok
 
-	} else if (report) {
-		if (_ack_state == UBX_ACK_GOT_NAK) {
-			UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
+    } else if (report) {
+        if (_ack_state == UBX_ACK_GOT_NAK) {
+            UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
 
-		} else {
-			UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
-		}
-	}
+        } else {
+            UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
+        }
+    }
 
-	_ack_state = UBX_ACK_IDLE;
-	return ret;
+    _ack_state = UBX_ACK_IDLE;
+    return ret;
 }
 
-int	// -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
-GPSDriverUBX::receive(unsigned timeout)
-{
-	uint8_t buf[GPS_READ_BUFFER_SIZE];
-
-	/* timeout additional to poll */
-	gps_abstime time_started = gps_absolute_time();
-
-	int handled = 0;
-
-	while (true) {
-		bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;
-
-		/* return success if ready */
-		if (ready_to_return) {
-			_got_posllh = false;
-			_got_velned = false;
-			return handled;
-		}
-
-		/* Wait for only UBX_PACKET_TIMEOUT if something already received. */
-		int ret = read(buf, sizeof(buf), (_got_posllh || _got_velned) ? UBX_PACKET_TIMEOUT : timeout);
-
-		if (ret < 0) {
-			/* something went wrong when polling or reading */
-			UBX_WARN("ubx poll_or_read err");
-			return -1;
-
-		} else if (ret > 0) {
-			//UBX_DEBUG("read %d bytes", ret);
-
-			/* pass received bytes to the packet decoder */
-			for (int i = 0; i < ret; i++) {
-				handled |= parseChar(buf[i]);
-				//UBX_DEBUG("parsed %d: 0x%x", i, buf[i]);
-			}
-
-			if (_interface == Interface::SPI) {
-				if (buf[ret - 1] == 0xff) {
-					if (ready_to_return) {
-						_got_posllh = false;
-						_got_velned = false;
-						return handled;
-					}
-				}
-			}
-		}
-
-		/* abort after timeout if no useful packets received */
-		if (time_started + timeout * 1000 < gps_absolute_time()) {
-			UBX_DEBUG("timed out, returning");
-			return -1;
-		}
-	}
+int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
+GPSDriverUBX::receive(unsigned timeout) {
+    uint8_t buf[GPS_READ_BUFFER_SIZE];
+
+    /* timeout additional to poll */
+    gps_abstime time_started = gps_absolute_time();
+
+    int handled = 0;
+
+    while (true) {
+        bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;
+
+        /* return success if ready */
+        if (ready_to_return) {
+            _got_posllh = false;
+            _got_velned = false;
+            return handled;
+        }
+
+        /* Wait for only UBX_PACKET_TIMEOUT if something already received. */
+        int ret = read(buf, sizeof(buf), (_got_posllh || _got_velned) ? UBX_PACKET_TIMEOUT : timeout);
+
+        if (ret < 0) {
+            /* something went wrong when polling or reading */
+            UBX_WARN("ubx poll_or_read err");
+            return -1;
+
+        } else if (ret > 0) {
+            //UBX_DEBUG("read %d bytes", ret);
+
+            /* pass received bytes to the packet decoder */
+            for (int i = 0; i < ret; i++) {
+                handled |= parseChar(buf[i]);
+                //UBX_DEBUG("parsed %d: 0x%x", i, buf[i]);
+            }
+
+            if (_interface == Interface::SPI) {
+                if (buf[ret - 1] == 0xff) {
+                    if (ready_to_return) {
+                        _got_posllh = false;
+                        _got_velned = false;
+                        return handled;
+                    }
+                }
+            }
+        }
+
+        /* abort after timeout if no useful packets received */
+        if (time_started + timeout * 1000 < gps_absolute_time()) {
+            UBX_DEBUG("timed out, returning");
+            return -1;
+        }
+    }
 }
 
-int	// 0 = decoding, 1 = message handled, 2 = sat info message handled
-GPSDriverUBX::parseChar(const uint8_t b)
-{
-	int ret = 0;
-
-	switch (_decode_state) {
-
-	/* Expecting Sync1 */
-	case UBX_DECODE_SYNC1:
-		if (b == UBX_SYNC1) {	// Sync1 found --> expecting Sync2
-			UBX_TRACE_PARSER("A");
-			_decode_state = UBX_DECODE_SYNC2;
-
-		} else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
-			UBX_TRACE_PARSER("RTCM");
-			_decode_state = UBX_DECODE_RTCM3;
-			_rtcm_parsing->addByte(b);
-		}
-
-		break;
-
-	/* Expecting Sync2 */
-	case UBX_DECODE_SYNC2:
-		if (b == UBX_SYNC2) {	// Sync2 found --> expecting Class
-			UBX_TRACE_PARSER("B");
-			_decode_state = UBX_DECODE_CLASS;
-
-		} else {		// Sync1 not followed by Sync2: reset parser
-			decodeInit();
-		}
-
-		break;
-
-	/* Expecting Class */
-	case UBX_DECODE_CLASS:
-		UBX_TRACE_PARSER("C");
-		addByteToChecksum(b);  // checksum is calculated for everything except Sync and Checksum bytes
-		_rx_msg = b;
-		_decode_state = UBX_DECODE_ID;
-		break;
-
-	/* Expecting ID */
-	case UBX_DECODE_ID:
-		UBX_TRACE_PARSER("D");
-		addByteToChecksum(b);
-		_rx_msg |= b << 8;
-		_decode_state = UBX_DECODE_LENGTH1;
-		break;
-
-	/* Expecting first length byte */
-	case UBX_DECODE_LENGTH1:
-		UBX_TRACE_PARSER("E");
-		addByteToChecksum(b);
-		_rx_payload_length = b;
-		_decode_state = UBX_DECODE_LENGTH2;
-		break;
-
-	/* Expecting second length byte */
-	case UBX_DECODE_LENGTH2:
-		UBX_TRACE_PARSER("F");
-		addByteToChecksum(b);
-		_rx_payload_length |= b << 8;	// calculate payload size
-
-		if (payloadRxInit() != 0) {	// start payload reception
-			// payload will not be handled, discard message
-			decodeInit();
-
-		} else {
-			_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
-		}
-
-		break;
-
-	/* Expecting payload */
-	case UBX_DECODE_PAYLOAD:
-		UBX_TRACE_PARSER(".");
-		addByteToChecksum(b);
-
-		switch (_rx_msg) {
-		case UBX_MSG_NAV_SAT:
-			ret = payloadRxAddNavSat(b);	// add a NAV-SAT payload byte
-			break;
-
-		case UBX_MSG_NAV_SVINFO:
-			ret = payloadRxAddNavSvinfo(b);	// add a NAV-SVINFO payload byte
-			break;
-
-		case UBX_MSG_MON_VER:
-			ret = payloadRxAddMonVer(b);	// add a MON-VER payload byte
-			break;
-
-		default:
-			ret = payloadRxAdd(b);		// add a payload byte
-			break;
-		}
-
-		if (ret < 0) {
-			// payload not handled, discard message
-			decodeInit();
-
-		} else if (ret > 0) {
-			// payload complete, expecting checksum
-			_decode_state = UBX_DECODE_CHKSUM1;
-
-		} else {
-			// expecting more payload, stay in state UBX_DECODE_PAYLOAD
-		}
-
-		ret = 0;
-		break;
-
-	/* Expecting first checksum byte */
-	case UBX_DECODE_CHKSUM1:
-		if (_rx_ck_a != b) {
-			UBX_DEBUG("ubx checksum err");
-			decodeInit();
-
-		} else {
-			_decode_state = UBX_DECODE_CHKSUM2;
-		}
-
-		break;
-
-	/* Expecting second checksum byte */
-	case UBX_DECODE_CHKSUM2:
-		if (_rx_ck_b != b) {
-			UBX_DEBUG("ubx checksum err");
-
-		} else {
-			ret = payloadRxDone();	// finish payload processing
-		}
-
-		decodeInit();
-		break;
-
-	case UBX_DECODE_RTCM3:
-		if (_rtcm_parsing->addByte(b)) {
-			//UBX_DEBUG("got RTCM message with length %i", static_cast<int>(_rtcm_parsing->messageLength()));
-			gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
-			decodeInit();
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
+int // 0 = decoding, 1 = message handled, 2 = sat info message handled
+GPSDriverUBX::parseChar(const uint8_t b) {
+    int ret = 0;
+
+    switch (_decode_state) {
+    /* Expecting Sync1 */
+    case UBX_DECODE_SYNC1:
+        if (b == UBX_SYNC1) { // Sync1 found --> expecting Sync2
+            UBX_TRACE_PARSER("A");
+            _decode_state = UBX_DECODE_SYNC2;
+
+        } else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
+            UBX_TRACE_PARSER("RTCM");
+            _decode_state = UBX_DECODE_RTCM3;
+            _rtcm_parsing->addByte(b);
+        }
+
+        break;
+
+    /* Expecting Sync2 */
+    case UBX_DECODE_SYNC2:
+        if (b == UBX_SYNC2) { // Sync2 found --> expecting Class
+            UBX_TRACE_PARSER("B");
+            _decode_state = UBX_DECODE_CLASS;
+
+        } else { // Sync1 not followed by Sync2: reset parser
+            decodeInit();
+        }
+
+        break;
+
+    /* Expecting Class */
+    case UBX_DECODE_CLASS:
+        UBX_TRACE_PARSER("C");
+        addByteToChecksum(b); // checksum is calculated for everything except Sync and Checksum bytes
+        _rx_msg       = b;
+        _decode_state = UBX_DECODE_ID;
+        break;
+
+    /* Expecting ID */
+    case UBX_DECODE_ID:
+        UBX_TRACE_PARSER("D");
+        addByteToChecksum(b);
+        _rx_msg       |= b << 8;
+        _decode_state  = UBX_DECODE_LENGTH1;
+        break;
+
+    /* Expecting first length byte */
+    case UBX_DECODE_LENGTH1:
+        UBX_TRACE_PARSER("E");
+        addByteToChecksum(b);
+        _rx_payload_length = b;
+        _decode_state      = UBX_DECODE_LENGTH2;
+        break;
+
+    /* Expecting second length byte */
+    case UBX_DECODE_LENGTH2:
+        UBX_TRACE_PARSER("F");
+        addByteToChecksum(b);
+        _rx_payload_length |= b << 8; // calculate payload size
+
+        if (payloadRxInit() != 0) {   // start payload reception
+            // payload will not be handled, discard message
+            decodeInit();
+
+        } else {
+            _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
+        }
+
+        break;
+
+    /* Expecting payload */
+    case UBX_DECODE_PAYLOAD:
+        UBX_TRACE_PARSER(".");
+        addByteToChecksum(b);
+
+        switch (_rx_msg) {
+        case UBX_MSG_NAV_SAT:
+            ret = payloadRxAddNavSat(b); // add a NAV-SAT payload byte
+            break;
+
+        case UBX_MSG_NAV_SVINFO:
+            ret = payloadRxAddNavSvinfo(b); // add a NAV-SVINFO payload byte
+            break;
+
+        case UBX_MSG_MON_VER:
+            ret = payloadRxAddMonVer(b); // add a MON-VER payload byte
+            break;
+
+        default:
+            ret = payloadRxAdd(b); // add a payload byte
+            break;
+        }
+
+        if (ret < 0) {
+            // payload not handled, discard message
+            decodeInit();
+
+        } else if (ret > 0) {
+            // payload complete, expecting checksum
+            _decode_state = UBX_DECODE_CHKSUM1;
+
+        } else {
+            // expecting more payload, stay in state UBX_DECODE_PAYLOAD
+        }
+
+        ret = 0;
+        break;
+
+    /* Expecting first checksum byte */
+    case UBX_DECODE_CHKSUM1:
+        if (_rx_ck_a != b) {
+            UBX_DEBUG("ubx checksum err");
+            decodeInit();
+
+        } else {
+            _decode_state = UBX_DECODE_CHKSUM2;
+        }
+
+        break;
+
+    /* Expecting second checksum byte */
+    case UBX_DECODE_CHKSUM2:
+        if (_rx_ck_b != b) {
+            UBX_DEBUG("ubx checksum err");
+
+        } else {
+            ret = payloadRxDone(); // finish payload processing
+        }
+
+        decodeInit();
+        break;
+
+    case UBX_DECODE_RTCM3:
+        if (_rtcm_parsing->addByte(b)) {
+            //UBX_DEBUG("got RTCM message with length %i", static_cast<int>(_rtcm_parsing->messageLength()));
+            gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
+            decodeInit();
+        }
+
+        break;
+
+    default:
+        break;
+    }
+
+    return ret;
 }
 
 /**
  * Start payload rx
  */
-int	// -1 = abort, 0 = continue
-GPSDriverUBX::payloadRxInit()
-{
-	int ret = 0;
+int // -1 = abort, 0 = continue
+GPSDriverUBX::payloadRxInit() {
+    int ret = 0;
 
-	_rx_state = UBX_RXMSG_HANDLE;	// handle by default
+    _rx_state = UBX_RXMSG_HANDLE; // handle by default
 
-	switch (_rx_msg) {
-	case UBX_MSG_NAV_PVT:
-		if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7)		/* u-blox 7 msg format */
-		    && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) {	/* u-blox 8+ msg format */
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    switch (_rx_msg) {
+    case UBX_MSG_NAV_PVT:
+        if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7)       /* u-blox 7 msg format */
+            && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		} else if (!_use_nav_pvt) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if not using NAV-PVT
-		}
+        } else if (!_use_nav_pvt) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_INF_DEBUG:
-	case UBX_MSG_INF_ERROR:
-	case UBX_MSG_INF_NOTICE:
-	case UBX_MSG_INF_WARNING:
-		if (_rx_payload_length >= sizeof(ubx_buf_t)) {
-			_rx_payload_length = sizeof(ubx_buf_t) - 1; //avoid buffer overflow
-		}
+    case UBX_MSG_INF_DEBUG:
+    case UBX_MSG_INF_ERROR:
+    case UBX_MSG_INF_NOTICE:
+    case UBX_MSG_INF_WARNING:
+        if (_rx_payload_length >= sizeof(ubx_buf_t)) {
+            _rx_payload_length = sizeof(ubx_buf_t) - 1; //avoid buffer overflow
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_NAV_POSLLH:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_MSG_NAV_POSLLH:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		} else if (_use_nav_pvt) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if using NAV-PVT instead
-		}
+        } else if (_use_nav_pvt) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_NAV_SOL:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_MSG_NAV_SOL:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		} else if (_use_nav_pvt) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if using NAV-PVT instead
-		}
+        } else if (_use_nav_pvt) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_NAV_STATUS:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_status_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_MSG_NAV_STATUS:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_status_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-		}
+        break;
 
-		break;
+    case UBX_MSG_NAV_DOP:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_dop_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-	case UBX_MSG_NAV_DOP:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_dop_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        break;
 
-		}
+    case UBX_MSG_NAV_RELPOSNED:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_relposned_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		break;
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-	case UBX_MSG_NAV_RELPOSNED:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_relposned_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        break;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+    case UBX_MSG_NAV_TIMEUTC:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		}
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		break;
+        } else if (_use_nav_pvt) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
+        }
 
-	case UBX_MSG_NAV_TIMEUTC:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        break;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+    case UBX_MSG_NAV_SAT:
+    case UBX_MSG_NAV_SVINFO:
+        if (_satellite_info == nullptr) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
 
-		} else if (_use_nav_pvt) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if using NAV-PVT instead
-		}
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		break;
+        } else {
+            memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
+        }
 
-	case UBX_MSG_NAV_SAT:
-	case UBX_MSG_NAV_SVINFO:
-		if (_satellite_info == nullptr) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if sat info not requested
+        break;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+    case UBX_MSG_NAV_SVIN:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_svin_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else {
-			memset(_satellite_info, 0, sizeof(*_satellite_info));        // initialize sat info
-		}
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_NAV_SVIN:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_svin_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_MSG_NAV_VELNED:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
 
-		}
+        } else if (_use_nav_pvt) {
+            _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_NAV_VELNED:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_MSG_MON_VER:
+        break; // unconditionally handle this message
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
+    case UBX_MSG_MON_HW:
+        if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))       /* u-blox 6 msg format */
+            && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		} else if (_use_nav_pvt) {
-			_rx_state = UBX_RXMSG_DISABLE;        // disable if using NAV-PVT instead
-		}
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_MON_VER:
-		break;		// unconditionally handle this message
+    case UBX_MSG_MON_RF:
+        if (_rx_payload_length < sizeof(ubx_payload_rx_mon_rf_t) || (_rx_payload_length - 4) % sizeof(ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t) != 0) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-	case UBX_MSG_MON_HW:
-		if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))	/* u-blox 6 msg format */
-		    && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) {	/* u-blox 7+ msg format */
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        } else if (!_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
+        }
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
-		}
+        break;
 
-		break;
+    case UBX_MSG_ACK_ACK:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-	case UBX_MSG_MON_RF:
-		if (_rx_payload_length < sizeof(ubx_payload_rx_mon_rf_t) ||
-		    (_rx_payload_length - 4) % sizeof(ubx_payload_rx_mon_rf_t::ubx_payload_rx_mon_rf_block_t) != 0) {
+        } else if (_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
+        }
 
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        break;
 
-		} else if (!_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if not _configured
-		}
+    case UBX_MSG_ACK_NAK:
+        if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) {
+            _rx_state = UBX_RXMSG_ERROR_LENGTH;
 
-		break;
+        } else if (_configured) {
+            _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
+        }
 
-	case UBX_MSG_ACK_ACK:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+        break;
 
-		} else if (_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if _configured
-		}
+    default:
+        _rx_state = UBX_RXMSG_DISABLE; // disable all other messages
+        break;
+    }
 
-		break;
+    switch (_rx_state) {
+    case UBX_RXMSG_HANDLE: // handle message
+    case UBX_RXMSG_IGNORE: // ignore message but don't report error
+        ret = 0;
+        break;
 
-	case UBX_MSG_ACK_NAK:
-		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) {
-			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+    case UBX_RXMSG_DISABLE: // disable unexpected messages
+        UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
 
-		} else if (_configured) {
-			_rx_state = UBX_RXMSG_IGNORE;        // ignore if _configured
-		}
+        if (_proto_ver_27_or_higher) {
+            uint32_t key_id = 0;
 
-		break;
+            switch (_rx_msg) { // we cannot infer the config Key ID from _rx_msg for protocol version 27+
+            case UBX_MSG_RXM_RAWX:
+                key_id = UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C;
+                break;
 
-	default:
-		_rx_state = UBX_RXMSG_DISABLE;	// disable all other messages
-		break;
-	}
+            case UBX_MSG_RXM_SFRBX:
+                key_id = UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C;
+                break;
+            }
 
-	switch (_rx_state) {
-	case UBX_RXMSG_HANDLE:	// handle message
-	case UBX_RXMSG_IGNORE:	// ignore message but don't report error
-		ret = 0;
-		break;
+            if (key_id != 0) {
+                gps_abstime t = gps_absolute_time();
 
-	case UBX_RXMSG_DISABLE:	// disable unexpected messages
-		UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+                if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL && _configured) {
+                    /* don't attempt for every message to disable, some might not be disabled */
+                    _disable_cmd_last = t;
+                    UBX_DEBUG("ubx disabling msg 0x%04x (0x%04x)", SWAP16((unsigned)_rx_msg), (uint16_t)key_id);
 
-		if (_proto_ver_27_or_higher) {
-			uint32_t key_id = 0;
+                    // this will overwrite _buf, which is fine, as we'll return -1 and abort further parsing
+                    int cfg_valset_msg_size = initCfgValset();
+                    cfgValsetPort(key_id, 0, cfg_valset_msg_size);
+                    sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size);
+                }
+            }
 
-			switch (_rx_msg) { // we cannot infer the config Key ID from _rx_msg for protocol version 27+
-			case UBX_MSG_RXM_RAWX:
-				key_id = UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C;
-				break;
+        } else {
+            gps_abstime t = gps_absolute_time();
 
-			case UBX_MSG_RXM_SFRBX:
-				key_id = UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C;
-				break;
-			}
+            if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
+                /* don't attempt for every message to disable, some might not be disabled */
+                _disable_cmd_last = t;
+                UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
 
-			if (key_id != 0) {
-				gps_abstime t = gps_absolute_time();
+                configureMessageRate(_rx_msg, 0);
+            }
+        }
 
-				if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL && _configured) {
-					/* don't attempt for every message to disable, some might not be disabled */
-					_disable_cmd_last = t;
-					UBX_DEBUG("ubx disabling msg 0x%04x (0x%04x)", SWAP16((unsigned)_rx_msg), (uint16_t)key_id);
+        ret = -1; // return error, abort handling this message
+        break;
 
-					// this will overwrite _buf, which is fine, as we'll return -1 and abort further parsing
-					int cfg_valset_msg_size = initCfgValset();
-					cfgValsetPort(key_id, 0, cfg_valset_msg_size);
-					sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size);
-				}
-			}
+    case UBX_RXMSG_ERROR_LENGTH: // error: invalid length
+        UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+        ret = -1;                // return error, abort handling this message
+        break;
 
-		} else {
-			gps_abstime t = gps_absolute_time();
+    default:      // invalid message state
+        UBX_WARN("ubx internal err1");
+        ret = -1; // return error, abort handling this message
+        break;
+    }
 
-			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
-				/* don't attempt for every message to disable, some might not be disabled */
-				_disable_cmd_last = t;
-				UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
-
-				configureMessageRate(_rx_msg, 0);
-			}
-		}
-
-		ret = -1;	// return error, abort handling this message
-		break;
-
-	case UBX_RXMSG_ERROR_LENGTH:	// error: invalid length
-		UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
-		ret = -1;	// return error, abort handling this message
-		break;
-
-	default:	// invalid message state
-		UBX_WARN("ubx internal err1");
-		ret = -1;	// return error, abort handling this message
-		break;
-	}
-
-	return ret;
+    return ret;
 }
 
 /**
  * Add payload rx byte
  */
-int	// -1 = error, 0 = ok, 1 = payload completed
-GPSDriverUBX::payloadRxAdd(const uint8_t b)
-{
-	int ret = 0;
-	uint8_t *p_buf = (uint8_t *)&_buf;
+int // -1 = error, 0 = ok, 1 = payload completed
+GPSDriverUBX::payloadRxAdd(const uint8_t b) {
+    int      ret   = 0;
+    uint8_t *p_buf = (uint8_t *)&_buf;
 
-	p_buf[_rx_payload_index] = b;
+    p_buf[_rx_payload_index] = b;
 
-	if (++_rx_payload_index >= _rx_payload_length) {
-		ret = 1;	// payload received completely
-	}
+    if (++_rx_payload_index >= _rx_payload_length) {
+        ret = 1; // payload received completely
+    }
 
-	return ret;
+    return ret;
 }
 
-int	// -1 = error, 0 = ok, 1 = payload completed
-GPSDriverUBX::payloadRxAddNavSat(const uint8_t b)
-{
-	int ret = 0;
-	uint8_t *p_buf = (uint8_t *)&_buf;
-
-	if (_rx_payload_index < sizeof(ubx_payload_rx_nav_sat_part1_t)) {
-		// Fill Part 1 buffer
-		p_buf[_rx_payload_index] = b;
-
-	} else {
-		if (_rx_payload_index == sizeof(ubx_payload_rx_nav_sat_part1_t)) {
-			// Part 1 complete: decode Part 1 buffer
-			_satellite_info->count = MIN(_buf.payload_rx_nav_sat_part1.numSvs, satellite_info_s::SAT_INFO_MAX_SATELLITES);
-			UBX_TRACE_SVINFO("SAT len %u  numCh %u", (unsigned)_rx_payload_length,
-					 (unsigned)_buf.payload_rx_nav_sat_part1.numSvs);
-		}
-
-		if (_rx_payload_index < sizeof(ubx_payload_rx_nav_sat_part1_t) + _satellite_info->count * sizeof(
-			    ubx_payload_rx_nav_sat_part2_t)) {
-			// Still room in _satellite_info: fill Part 2 buffer
-			unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_sat_part1_t)) % sizeof(
-						     ubx_payload_rx_nav_sat_part2_t);
-			p_buf[buf_index] = b;
-
-			if (buf_index == sizeof(ubx_payload_rx_nav_sat_part2_t) - 1) {
-				// Part 2 complete: decode Part 2 buffer
-				unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_sat_part1_t)) /
-						     sizeof(ubx_payload_rx_nav_sat_part2_t);
-
-				// convert gnssId:svId to a 8 bit number (use svId numbering from NAV-SVINFO)
-				uint8_t ubx_sat_gnssId = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.gnssId);
-				uint8_t ubx_sat_svId = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.svId);
-
-				uint8_t svinfo_svid = 255;
-
-				switch (ubx_sat_gnssId) {
-				case 0:  // GPS: G1-G23 -> 1-32
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 32) {
-						svinfo_svid = ubx_sat_svId;
-					}
-
-					break;
-
-				case 1:  // SBAS: S120-S158 -> 120-158
-					if (ubx_sat_svId >= 120 && ubx_sat_svId <= 158) {
-						svinfo_svid = ubx_sat_svId;
-					}
-
-					break;
-
-				case 2:  // Galileo: E1-E36 -> 211-246
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 36) {
-						svinfo_svid = ubx_sat_svId + 210;
-					}
-
-					break;
-
-				case 3:  // BeiDou: B1-B37 -> 159-163,33-64
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 4) {
-						svinfo_svid = ubx_sat_svId + 158;
-
-					} else if (ubx_sat_svId >= 5 && ubx_sat_svId <= 37) {
-						svinfo_svid = ubx_sat_svId + 28;
-					}
-
-					break;
-
-				case 4:  // IMES: I1-I10 -> 173-182
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 10) {
-						svinfo_svid = ubx_sat_svId + 172;
-					}
-
-					break;
-
-				case 5:  // QZSS: Q1-A10 -> 193-202
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 10) {
-						svinfo_svid = ubx_sat_svId + 192;
-					}
-
-					break;
-
-				case 6:  // GLONASS: R1-R32 -> 65-96, R? -> 255
-					if (ubx_sat_svId >= 1 && ubx_sat_svId <= 32) {
-						svinfo_svid = ubx_sat_svId + 64;
-					}
-
-					break;
-				}
-
-				_satellite_info->svid[sat_index]	  = svinfo_svid;
-				_satellite_info->used[sat_index]	  = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.flags & 0x01);
-				_satellite_info->elevation[sat_index] = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.elev);
-				_satellite_info->azimuth[sat_index]	  = static_cast<uint8_t>(static_cast<float>(_buf.payload_rx_nav_sat_part2.azim) *
-						255.0f / 360.0f);
-				_satellite_info->snr[sat_index]		  = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.cno);
-				_satellite_info->prn[sat_index]		  = svinfo_svid;
-				UBX_TRACE_SVINFO("SAT #%02u  svid %3u  used %u  elevation %3u  azimuth %3u  snr %3u  prn %3u",
-						 static_cast<unsigned>(sat_index + 1),
-						 static_cast<unsigned>(_satellite_info->svid[sat_index]),
-						 static_cast<unsigned>(_satellite_info->used[sat_index]),
-						 static_cast<unsigned>(_satellite_info->elevation[sat_index]),
-						 static_cast<unsigned>(_satellite_info->azimuth[sat_index]),
-						 static_cast<unsigned>(_satellite_info->snr[sat_index]),
-						 static_cast<unsigned>(_satellite_info->prn[sat_index])
-						);
-			}
-		}
-	}
-
-	if (++_rx_payload_index >= _rx_payload_length) {
-		ret = 1;	// payload received completely
-	}
-
-	return ret;
+int // -1 = error, 0 = ok, 1 = payload completed
+GPSDriverUBX::payloadRxAddNavSat(const uint8_t b) {
+    int      ret   = 0;
+    uint8_t *p_buf = (uint8_t *)&_buf;
+
+    if (_rx_payload_index < sizeof(ubx_payload_rx_nav_sat_part1_t)) {
+        // Fill Part 1 buffer
+        p_buf[_rx_payload_index] = b;
+
+    } else {
+        if (_rx_payload_index == sizeof(ubx_payload_rx_nav_sat_part1_t)) {
+            // Part 1 complete: decode Part 1 buffer
+            _satellite_info->count = MIN(_buf.payload_rx_nav_sat_part1.numSvs, satellite_info_s::SAT_INFO_MAX_SATELLITES);
+            UBX_TRACE_SVINFO("SAT len %u  numCh %u", (unsigned)_rx_payload_length,
+                             (unsigned)_buf.payload_rx_nav_sat_part1.numSvs);
+        }
+
+        if (_rx_payload_index < sizeof(ubx_payload_rx_nav_sat_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_sat_part2_t)) {
+            // Still room in _satellite_info: fill Part 2 buffer
+            unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_sat_part1_t)) % sizeof(ubx_payload_rx_nav_sat_part2_t);
+            p_buf[buf_index]   = b;
+
+            if (buf_index == sizeof(ubx_payload_rx_nav_sat_part2_t) - 1) {
+                // Part 2 complete: decode Part 2 buffer
+                unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_sat_part1_t)) / sizeof(ubx_payload_rx_nav_sat_part2_t);
+
+                // convert gnssId:svId to a 8 bit number (use svId numbering from NAV-SVINFO)
+                uint8_t ubx_sat_gnssId = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.gnssId);
+                uint8_t ubx_sat_svId   = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.svId);
+
+                uint8_t svinfo_svid = 255;
+
+                switch (ubx_sat_gnssId) {
+                case 0: // GPS: G1-G23 -> 1-32
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 32) {
+                        svinfo_svid = ubx_sat_svId;
+                    }
+
+                    break;
+
+                case 1: // SBAS: S120-S158 -> 120-158
+                    if (ubx_sat_svId >= 120 && ubx_sat_svId <= 158) {
+                        svinfo_svid = ubx_sat_svId;
+                    }
+
+                    break;
+
+                case 2: // Galileo: E1-E36 -> 211-246
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 36) {
+                        svinfo_svid = ubx_sat_svId + 210;
+                    }
+
+                    break;
+
+                case 3: // BeiDou: B1-B37 -> 159-163,33-64
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 4) {
+                        svinfo_svid = ubx_sat_svId + 158;
+
+                    } else if (ubx_sat_svId >= 5 && ubx_sat_svId <= 37) {
+                        svinfo_svid = ubx_sat_svId + 28;
+                    }
+
+                    break;
+
+                case 4: // IMES: I1-I10 -> 173-182
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 10) {
+                        svinfo_svid = ubx_sat_svId + 172;
+                    }
+
+                    break;
+
+                case 5: // QZSS: Q1-A10 -> 193-202
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 10) {
+                        svinfo_svid = ubx_sat_svId + 192;
+                    }
+
+                    break;
+
+                case 6: // GLONASS: R1-R32 -> 65-96, R? -> 255
+                    if (ubx_sat_svId >= 1 && ubx_sat_svId <= 32) {
+                        svinfo_svid = ubx_sat_svId + 64;
+                    }
+
+                    break;
+                }
+
+                _satellite_info->svid[sat_index]      = svinfo_svid;
+                _satellite_info->used[sat_index]      = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.flags & 0x01);
+                _satellite_info->elevation[sat_index] = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.elev);
+                _satellite_info->azimuth[sat_index]   = static_cast<uint8_t>(static_cast<float>(_buf.payload_rx_nav_sat_part2.azim) * 255.0f / 360.0f);
+                _satellite_info->snr[sat_index]       = static_cast<uint8_t>(_buf.payload_rx_nav_sat_part2.cno);
+                _satellite_info->prn[sat_index]       = svinfo_svid;
+                UBX_TRACE_SVINFO("SAT #%02u  svid %3u  used %u  elevation %3u  azimuth %3u  snr %3u  prn %3u",
+                                 static_cast<unsigned>(sat_index + 1),
+                                 static_cast<unsigned>(_satellite_info->svid[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->used[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->elevation[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->azimuth[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->snr[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->prn[sat_index]));
+            }
+        }
+    }
+
+    if (++_rx_payload_index >= _rx_payload_length) {
+        ret = 1; // payload received completely
+    }
+
+    return ret;
 }
 
 /**
  * Add NAV-SVINFO payload rx byte
  */
-int	// -1 = error, 0 = ok, 1 = payload completed
-GPSDriverUBX::payloadRxAddNavSvinfo(const uint8_t b)
-{
-	int ret = 0;
-	uint8_t *p_buf = (uint8_t *)&_buf;
-
-	if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
-		// Fill Part 1 buffer
-		p_buf[_rx_payload_index] = b;
-
-	} else {
-		if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
-			// Part 1 complete: decode Part 1 buffer
-			_satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES);
-			UBX_TRACE_SVINFO("SVINFO len %u  numCh %u", (unsigned)_rx_payload_length,
-					 (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
-		}
-
-		if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(
-			    ubx_payload_rx_nav_svinfo_part2_t)) {
-			// Still room in _satellite_info: fill Part 2 buffer
-			unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(
-						     ubx_payload_rx_nav_svinfo_part2_t);
-			p_buf[buf_index] = b;
-
-			if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
-				// Part 2 complete: decode Part 2 buffer
-				unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) /
-						     sizeof(ubx_payload_rx_nav_svinfo_part2_t);
-				_satellite_info->svid[sat_index]      = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.svid);
-				_satellite_info->used[sat_index]      = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
-				_satellite_info->elevation[sat_index] = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.elev);
-				_satellite_info->azimuth[sat_index]   = static_cast<uint8_t>(static_cast<float>(_buf.payload_rx_nav_svinfo_part2.azim) *
-									255.0f / 360.0f);
-				_satellite_info->snr[sat_index]       = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.cno);
-				_satellite_info->prn[sat_index]       = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.svid);
-
-				UBX_TRACE_SVINFO("SVINFO #%02u  svid %3u  used %u  elevation %3u  azimuth %3u  snr %3u  prn %3u",
-						 static_cast<unsigned>(sat_index + 1),
-						 static_cast<unsigned>(_satellite_info->svid[sat_index]),
-						 static_cast<unsigned>(_satellite_info->used[sat_index]),
-						 static_cast<unsigned>(_satellite_info->elevation[sat_index]),
-						 static_cast<unsigned>(_satellite_info->azimuth[sat_index]),
-						 static_cast<unsigned>(_satellite_info->snr[sat_index]),
-						 static_cast<unsigned>(_satellite_info->prn[sat_index])
-						);
-			}
-		}
-	}
-
-	if (++_rx_payload_index >= _rx_payload_length) {
-		ret = 1;	// payload received completely
-	}
-
-	return ret;
+int // -1 = error, 0 = ok, 1 = payload completed
+GPSDriverUBX::payloadRxAddNavSvinfo(const uint8_t b) {
+    int      ret   = 0;
+    uint8_t *p_buf = (uint8_t *)&_buf;
+
+    if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+        // Fill Part 1 buffer
+        p_buf[_rx_payload_index] = b;
+
+    } else {
+        if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+            // Part 1 complete: decode Part 1 buffer
+            _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES);
+            UBX_TRACE_SVINFO("SVINFO len %u  numCh %u", (unsigned)_rx_payload_length,
+                             (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
+        }
+
+        if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
+            // Still room in _satellite_info: fill Part 2 buffer
+            unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+            p_buf[buf_index]   = b;
+
+            if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
+                // Part 2 complete: decode Part 2 buffer
+                unsigned sat_index                    = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+                _satellite_info->svid[sat_index]      = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.svid);
+                _satellite_info->used[sat_index]      = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
+                _satellite_info->elevation[sat_index] = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.elev);
+                _satellite_info->azimuth[sat_index]   = static_cast<uint8_t>(static_cast<float>(_buf.payload_rx_nav_svinfo_part2.azim) * 255.0f / 360.0f);
+                _satellite_info->snr[sat_index]       = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.cno);
+                _satellite_info->prn[sat_index]       = static_cast<uint8_t>(_buf.payload_rx_nav_svinfo_part2.svid);
+
+                UBX_TRACE_SVINFO("SVINFO #%02u  svid %3u  used %u  elevation %3u  azimuth %3u  snr %3u  prn %3u",
+                                 static_cast<unsigned>(sat_index + 1),
+                                 static_cast<unsigned>(_satellite_info->svid[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->used[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->elevation[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->azimuth[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->snr[sat_index]),
+                                 static_cast<unsigned>(_satellite_info->prn[sat_index]));
+            }
+        }
+    }
+
+    if (++_rx_payload_index >= _rx_payload_length) {
+        ret = 1; // payload received completely
+    }
+
+    return ret;
 }
 
 /**
  * Add MON-VER payload rx byte
  */
-int	// -1 = error, 0 = ok, 1 = payload completed
-GPSDriverUBX::payloadRxAddMonVer(const uint8_t b)
-{
-	int ret = 0;
-	uint8_t *p_buf = (uint8_t *)&_buf;
-
-	if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
-		// Fill Part 1 buffer
-		p_buf[_rx_payload_index] = b;
-
-	} else {
-		if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
-			// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
-			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
-			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
-			UBX_DEBUG("VER hash 0x%08x", (uint16_t)_ubx_version);
-			UBX_DEBUG("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
-			UBX_DEBUG("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
-
-			// Device detection (See https://forum.u-blox.com/index.php/9432/need-help-decoding-ubx-mon-ver-hardware-string)
-			if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00040005",
-				    sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox5;
-
-			} else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00040007",
-					   sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox6;
-
-			} else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00070000",
-					   sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox7;
-
-			} else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00080000",
-					   sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox8;
-
-			} else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00190000",
-					   sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox9;
-
-			} else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "000A0000",
-					   sizeof(_buf.payload_rx_mon_ver_part1.hwVersion)) == 0) {
-				_board = Board::u_blox10;
-
-			} else {
-				UBX_WARN("unknown board hw: %s", _buf.payload_rx_mon_ver_part1.hwVersion);
-			}
-
-			UBX_DEBUG("detected board: %i", static_cast<int>(_board));
-		}
-
-		// fill Part 2 buffer
-		unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(
-					     ubx_payload_rx_mon_ver_part2_t);
-		p_buf[buf_index] = b;
-
-		if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
-			// Part 2 complete: decode Part 2 buffer
-			UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
-
-			// "FWVER=" Firmware of product category and version
-			const char *fwver_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "FWVER=");
-
-			if (fwver_str != nullptr) {
-				GPS_INFO("u-blox firmware version: %s", fwver_str + strlen("FWVER="));
-			}
-
-			// "PROTVER=" Supported protocol version.
-			const char *protver_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "PROTVER=");
-
-			if (protver_str != nullptr) {
-				GPS_INFO("u-blox protocol version: %s", protver_str + strlen("PROTVER="));
-			}
-
-			// "MOD=" Module identification. Set in production.
-			const char *mod_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "MOD=");
-
-			if (mod_str != nullptr) {
-				// in case of u-blox9 family, check if it's an F9P
-				if (_board == Board::u_blox9) {
-					if (strstr(mod_str, "F9P")) {
-						_board = Board::u_blox9_F9P;
-						UBX_DEBUG("F9P detected");
-					}
-				}
-
-				GPS_INFO("u-blox module: %s", mod_str + strlen("MOD="));
-			}
-		}
-	}
-
-	if (++_rx_payload_index >= _rx_payload_length) {
-		ret = 1;	// payload received completely
-	}
-
-	return ret;
+int // -1 = error, 0 = ok, 1 = payload completed
+GPSDriverUBX::payloadRxAddMonVer(const uint8_t b) {
+    int      ret   = 0;
+    uint8_t *p_buf = (uint8_t *)&_buf;
+
+    if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+        // Fill Part 1 buffer
+        p_buf[_rx_payload_index] = b;
+
+    } else {
+        if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+            // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
+            _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
+            _ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
+            UBX_DEBUG("VER hash 0x%08x", (uint16_t)_ubx_version);
+            UBX_DEBUG("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
+            UBX_DEBUG("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
+
+            // Device detection (See https://forum.u-blox.com/index.php/9432/need-help-decoding-ubx-mon-ver-hardware-string)
+            if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00040005",
+                        sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                == 0) {
+                _board = Board::u_blox5;
+
+            } else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00040007",
+                               sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                       == 0) {
+                _board = Board::u_blox6;
+
+            } else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00070000",
+                               sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                       == 0) {
+                _board = Board::u_blox7;
+
+            } else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00080000",
+                               sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                       == 0) {
+                _board = Board::u_blox8;
+
+            } else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "00190000",
+                               sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                       == 0) {
+                _board = Board::u_blox9;
+
+            } else if (strncmp((const char *)_buf.payload_rx_mon_ver_part1.hwVersion, "000A0000",
+                               sizeof(_buf.payload_rx_mon_ver_part1.hwVersion))
+                       == 0) {
+                _board = Board::u_blox10;
+
+            } else {
+                UBX_WARN("unknown board hw: %s", _buf.payload_rx_mon_ver_part1.hwVersion);
+            }
+
+            UBX_DEBUG("detected board: %i", static_cast<int>(_board));
+        }
+
+        // fill Part 2 buffer
+        unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
+        p_buf[buf_index]   = b;
+
+        if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
+            // Part 2 complete: decode Part 2 buffer
+            UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
+
+            // "FWVER=" Firmware of product category and version
+            const char *fwver_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "FWVER=");
+
+            if (fwver_str != nullptr) {
+                GPS_INFO("u-blox firmware version: %s", fwver_str + strlen("FWVER="));
+            }
+
+            // "PROTVER=" Supported protocol version.
+            const char *protver_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "PROTVER=");
+
+            if (protver_str != nullptr) {
+                GPS_INFO("u-blox protocol version: %s", protver_str + strlen("PROTVER="));
+            }
+
+            // "MOD=" Module identification. Set in production.
+            const char *mod_str = strstr((const char *)_buf.payload_rx_mon_ver_part2.extension, "MOD=");
+
+            if (mod_str != nullptr) {
+                // in case of u-blox9 family, check if it's an F9P
+                if (_board == Board::u_blox9) {
+                    if (strstr(mod_str, "F9P")) {
+                        _board = Board::u_blox9_F9P;
+                        UBX_DEBUG("F9P detected");
+                    }
+                }
+
+                GPS_INFO("u-blox module: %s", mod_str + strlen("MOD="));
+            }
+        }
+    }
+
+    if (++_rx_payload_index >= _rx_payload_length) {
+        ret = 1; // payload received completely
+    }
+
+    return ret;
 }
 
 /**
  * Finish payload rx
  */
-int	// 0 = no message handled, 1 = message handled, 2 = sat info message handled
-GPSDriverUBX::payloadRxDone()
-{
-	int ret = 0;
-
-	// return if no message handled
-	if (_rx_state != UBX_RXMSG_HANDLE) {
-		return ret;
-	}
+int // 0 = no message handled, 1 = message handled, 2 = sat info message handled
+GPSDriverUBX::payloadRxDone() {
+    int ret = 0;
 
-	// handle message
-	switch (_rx_msg) {
+    // return if no message handled
+    if (_rx_state != UBX_RXMSG_HANDLE) {
+        return ret;
+    }
 
-	case UBX_MSG_NAV_PVT:
-		UBX_TRACE_RXMSG("Rx NAV-PVT");
+    // handle message
+    switch (_rx_msg) {
+    case UBX_MSG_NAV_PVT:
+        UBX_TRACE_RXMSG("Rx NAV-PVT");
 
-		//Check if position fix flag is good
-		if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) {
-			_gps_position->fix_type		 = _buf.payload_rx_nav_pvt.fixType;
+        //Check if position fix flag is good
+        if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) {
+            _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
 
-			if (_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_DIFFSOLN) {
-				_gps_position->fix_type = 4; //DGPS
-			}
+            if (_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_DIFFSOLN) {
+                _gps_position->fix_type = 4; //DGPS
+            }
 
-			uint8_t carr_soln = _buf.payload_rx_nav_pvt.flags >> 6;
+            uint8_t carr_soln = _buf.payload_rx_nav_pvt.flags >> 6;
 
-			if (carr_soln == 1) {
-				_gps_position->fix_type = 5; //Float RTK
+            if (carr_soln == 1) {
+                _gps_position->fix_type = 5; //Float RTK
 
-			} else if (carr_soln == 2) {
-				_gps_position->fix_type = 6; //Fixed RTK
-			}
+            } else if (carr_soln == 2) {
+                _gps_position->fix_type = 6; //Fixed RTK
+            }
 
-			_gps_position->vel_ned_valid = true;
+            _gps_position->vel_ned_valid = true;
 
-		} else {
-			_gps_position->fix_type		 = 0;
-			_gps_position->vel_ned_valid = false;
-		}
+        } else {
+            _gps_position->fix_type      = 0;
+            _gps_position->vel_ned_valid = false;
+        }
 
-		_gps_position->satellites_used	= _buf.payload_rx_nav_pvt.numSV;
+        _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV;
 
-		_gps_position->lat		= _buf.payload_rx_nav_pvt.lat;
-		_gps_position->lon		= _buf.payload_rx_nav_pvt.lon;
-		_gps_position->alt		= _buf.payload_rx_nav_pvt.hMSL;
-		_gps_position->alt_ellipsoid	= _buf.payload_rx_nav_pvt.height;
+        _gps_position->lat           = _buf.payload_rx_nav_pvt.lat;
+        _gps_position->lon           = _buf.payload_rx_nav_pvt.lon;
+        _gps_position->alt           = _buf.payload_rx_nav_pvt.hMSL;
+        _gps_position->alt_ellipsoid = _buf.payload_rx_nav_pvt.height;
 
-		_gps_position->eph		= static_cast<float>(_buf.payload_rx_nav_pvt.hAcc) * 1e-3f;
-		_gps_position->epv		= static_cast<float>(_buf.payload_rx_nav_pvt.vAcc) * 1e-3f;
-		_gps_position->s_variance_m_s	= static_cast<float>(_buf.payload_rx_nav_pvt.sAcc) * 1e-3f;
+        _gps_position->eph            = static_cast<float>(_buf.payload_rx_nav_pvt.hAcc) * 1e-3f;
+        _gps_position->epv            = static_cast<float>(_buf.payload_rx_nav_pvt.vAcc) * 1e-3f;
+        _gps_position->s_variance_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.sAcc) * 1e-3f;
 
-		_gps_position->vel_m_s		= static_cast<float>(_buf.payload_rx_nav_pvt.gSpeed) * 1e-3f;
+        _gps_position->vel_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.gSpeed) * 1e-3f;
 
-		_gps_position->vel_n_m_s	= static_cast<float>(_buf.payload_rx_nav_pvt.velN) * 1e-3f;
-		_gps_position->vel_e_m_s	= static_cast<float>(_buf.payload_rx_nav_pvt.velE) * 1e-3f;
-		_gps_position->vel_d_m_s	= static_cast<float>(_buf.payload_rx_nav_pvt.velD) * 1e-3f;
+        _gps_position->vel_n_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.velN) * 1e-3f;
+        _gps_position->vel_e_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.velE) * 1e-3f;
+        _gps_position->vel_d_m_s = static_cast<float>(_buf.payload_rx_nav_pvt.velD) * 1e-3f;
 
-		_gps_position->cog_rad		= static_cast<float>(_buf.payload_rx_nav_pvt.headMot) * M_DEG_TO_RAD_F * 1e-5f;
-		_gps_position->c_variance_rad	= static_cast<float>(_buf.payload_rx_nav_pvt.headAcc) * M_DEG_TO_RAD_F * 1e-5f;
+        _gps_position->cog_rad        = static_cast<float>(_buf.payload_rx_nav_pvt.headMot) * M_DEG_TO_RAD_F * 1e-5f;
+        _gps_position->c_variance_rad = static_cast<float>(_buf.payload_rx_nav_pvt.headAcc) * M_DEG_TO_RAD_F * 1e-5f;
 
-		//Check if time and date fix flags are good
-		if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
-		    && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
-		    && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) {
-			/* convert to unix timestamp */
-			tm timeinfo{};
-			timeinfo.tm_year	= _buf.payload_rx_nav_pvt.year - 1900;
-			timeinfo.tm_mon		= _buf.payload_rx_nav_pvt.month - 1;
-			timeinfo.tm_mday	= _buf.payload_rx_nav_pvt.day;
-			timeinfo.tm_hour	= _buf.payload_rx_nav_pvt.hour;
-			timeinfo.tm_min		= _buf.payload_rx_nav_pvt.min;
-			timeinfo.tm_sec		= _buf.payload_rx_nav_pvt.sec;
+        //Check if time and date fix flags are good
+        if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
+            && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
+            && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) {
+            /* convert to unix timestamp */
+            tm timeinfo{};
+            timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
+            timeinfo.tm_mon  = _buf.payload_rx_nav_pvt.month - 1;
+            timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day;
+            timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour;
+            timeinfo.tm_min  = _buf.payload_rx_nav_pvt.min;
+            timeinfo.tm_sec  = _buf.payload_rx_nav_pvt.sec;
 
 #ifndef NO_MKTIME
-			time_t epoch = mktime(&timeinfo);
+            time_t epoch = mktime(&timeinfo);
 
-			if (epoch > GPS_EPOCH_SECS) {
-				// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-				// and control its drift. Since we rely on the HRT for our monotonic
-				// clock, updating it from time to time is safe.
+            if (epoch > GPS_EPOCH_SECS) {
+                // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+                // and control its drift. Since we rely on the HRT for our monotonic
+                // clock, updating it from time to time is safe.
 
-				timespec ts{};
-				ts.tv_sec = epoch;
-				ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
+                timespec ts{};
+                ts.tv_sec  = epoch;
+                ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
 
-				setClock(ts);
+                setClock(ts);
 
-				_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-				_gps_position->time_utc_usec += _buf.payload_rx_nav_pvt.nano / 1000;
+                _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+                _gps_position->time_utc_usec += _buf.payload_rx_nav_pvt.nano / 1000;
 
-			} else {
-				_gps_position->time_utc_usec = 0;
-			}
+            } else {
+                _gps_position->time_utc_usec = 0;
+            }
 
 #else
-			_gps_position->time_utc_usec = 0;
+            _gps_position->time_utc_usec = 0;
 #endif
-		}
+        }
 
-		_gps_position->timestamp = gps_absolute_time();
-		_last_timestamp_time = _gps_position->timestamp;
+        _gps_position->timestamp = gps_absolute_time();
+        _last_timestamp_time     = _gps_position->timestamp;
 
-		_rate_count_vel++;
-		_rate_count_lat_lon++;
+        _rate_count_vel++;
+        _rate_count_lat_lon++;
 
-		_got_posllh = true;
-		_got_velned = true;
+        _got_posllh = true;
+        _got_velned = true;
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_INF_DEBUG:
-	case UBX_MSG_INF_NOTICE: {
-			uint8_t *p_buf = (uint8_t *)&_buf;
-			p_buf[_rx_payload_length] = 0;
-			UBX_DEBUG("ubx msg: %s", p_buf);
-		}
-		break;
+    case UBX_MSG_INF_DEBUG:
+    case UBX_MSG_INF_NOTICE: {
+        uint8_t *p_buf            = (uint8_t *)&_buf;
+        p_buf[_rx_payload_length] = 0;
+        UBX_DEBUG("ubx msg: %s", p_buf);
+    } break;
 
-	case UBX_MSG_INF_ERROR:
-	case UBX_MSG_INF_WARNING: {
-			uint8_t *p_buf = (uint8_t *)&_buf;
-			p_buf[_rx_payload_length] = 0;
-			UBX_WARN("ubx msg: %s", p_buf);
-		}
-		break;
+    case UBX_MSG_INF_ERROR:
+    case UBX_MSG_INF_WARNING: {
+        uint8_t *p_buf            = (uint8_t *)&_buf;
+        p_buf[_rx_payload_length] = 0;
+        UBX_WARN("ubx msg: %s", p_buf);
+    } break;
 
-	case UBX_MSG_NAV_POSLLH:
-		UBX_TRACE_RXMSG("Rx NAV-POSLLH");
+    case UBX_MSG_NAV_POSLLH:
+        UBX_TRACE_RXMSG("Rx NAV-POSLLH");
 
-		_gps_position->lat	= _buf.payload_rx_nav_posllh.lat;
-		_gps_position->lon	= _buf.payload_rx_nav_posllh.lon;
-		_gps_position->alt	= _buf.payload_rx_nav_posllh.hMSL;
-		_gps_position->eph	= static_cast<float>(_buf.payload_rx_nav_posllh.hAcc) * 1e-3f; // from mm to m
-		_gps_position->epv	= static_cast<float>(_buf.payload_rx_nav_posllh.vAcc) * 1e-3f; // from mm to m
-		_gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height;
+        _gps_position->lat           = _buf.payload_rx_nav_posllh.lat;
+        _gps_position->lon           = _buf.payload_rx_nav_posllh.lon;
+        _gps_position->alt           = _buf.payload_rx_nav_posllh.hMSL;
+        _gps_position->eph           = static_cast<float>(_buf.payload_rx_nav_posllh.hAcc) * 1e-3f; // from mm to m
+        _gps_position->epv           = static_cast<float>(_buf.payload_rx_nav_posllh.vAcc) * 1e-3f; // from mm to m
+        _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height;
 
-		_gps_position->timestamp = gps_absolute_time();
+        _gps_position->timestamp = gps_absolute_time();
 
-		_rate_count_lat_lon++;
-		_got_posllh = true;
+        _rate_count_lat_lon++;
+        _got_posllh = true;
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_SOL:
-		UBX_TRACE_RXMSG("Rx NAV-SOL");
+    case UBX_MSG_NAV_SOL:
+        UBX_TRACE_RXMSG("Rx NAV-SOL");
 
-		_gps_position->fix_type		= _buf.payload_rx_nav_sol.gpsFix;
-		_gps_position->s_variance_m_s	= static_cast<float>(_buf.payload_rx_nav_sol.sAcc) * 1e-2f;	// from cm to m
-		_gps_position->satellites_used	= _buf.payload_rx_nav_sol.numSV;
+        _gps_position->fix_type        = _buf.payload_rx_nav_sol.gpsFix;
+        _gps_position->s_variance_m_s  = static_cast<float>(_buf.payload_rx_nav_sol.sAcc) * 1e-2f; // from cm to m
+        _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV;
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_STATUS:
-		UBX_TRACE_RXMSG("Rx NAV-STATUS");
+    case UBX_MSG_NAV_STATUS:
+        UBX_TRACE_RXMSG("Rx NAV-STATUS");
 
-		_gps_position->spoofing_state = (_buf.payload_rx_nav_status.flags2 & UBX_RX_NAV_STATUS_SPOOFDETSTATE_MASK) >>
-						UBX_RX_NAV_STATUS_SPOOFDETSTATE_SHIFT;
+        _gps_position->spoofing_state = (_buf.payload_rx_nav_status.flags2 & UBX_RX_NAV_STATUS_SPOOFDETSTATE_MASK) >> UBX_RX_NAV_STATUS_SPOOFDETSTATE_SHIFT;
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_DOP:
-		UBX_TRACE_RXMSG("Rx NAV-DOP");
+    case UBX_MSG_NAV_DOP:
+        UBX_TRACE_RXMSG("Rx NAV-DOP");
 
-		_gps_position->hdop		= _buf.payload_rx_nav_dop.hDOP * 0.01f;	// from cm to m
-		_gps_position->vdop		= _buf.payload_rx_nav_dop.vDOP * 0.01f;	// from cm to m
+        _gps_position->hdop = _buf.payload_rx_nav_dop.hDOP * 0.01f; // from cm to m
+        _gps_position->vdop = _buf.payload_rx_nav_dop.vDOP * 0.01f; // from cm to m
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_TIMEUTC:
-		UBX_TRACE_RXMSG("Rx NAV-TIMEUTC");
+    case UBX_MSG_NAV_TIMEUTC:
+        UBX_TRACE_RXMSG("Rx NAV-TIMEUTC");
 
-		if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) {
-			// convert to unix timestamp
-			tm timeinfo {};
-			timeinfo.tm_year	= _buf.payload_rx_nav_timeutc.year - 1900;
-			timeinfo.tm_mon		= _buf.payload_rx_nav_timeutc.month - 1;
-			timeinfo.tm_mday	= _buf.payload_rx_nav_timeutc.day;
-			timeinfo.tm_hour	= _buf.payload_rx_nav_timeutc.hour;
-			timeinfo.tm_min		= _buf.payload_rx_nav_timeutc.min;
-			timeinfo.tm_sec		= _buf.payload_rx_nav_timeutc.sec;
-			timeinfo.tm_isdst	= 0;
+        if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) {
+            // convert to unix timestamp
+            tm timeinfo{};
+            timeinfo.tm_year  = _buf.payload_rx_nav_timeutc.year - 1900;
+            timeinfo.tm_mon   = _buf.payload_rx_nav_timeutc.month - 1;
+            timeinfo.tm_mday  = _buf.payload_rx_nav_timeutc.day;
+            timeinfo.tm_hour  = _buf.payload_rx_nav_timeutc.hour;
+            timeinfo.tm_min   = _buf.payload_rx_nav_timeutc.min;
+            timeinfo.tm_sec   = _buf.payload_rx_nav_timeutc.sec;
+            timeinfo.tm_isdst = 0;
 #ifndef NO_MKTIME
-			time_t epoch = mktime(&timeinfo);
+            time_t epoch = mktime(&timeinfo);
 
-			// only set the time if it makes sense
+            // only set the time if it makes sense
 
-			if (epoch > GPS_EPOCH_SECS) {
-				// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
-				// and control its drift. Since we rely on the HRT for our monotonic
-				// clock, updating it from time to time is safe.
+            if (epoch > GPS_EPOCH_SECS) {
+                // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+                // and control its drift. Since we rely on the HRT for our monotonic
+                // clock, updating it from time to time is safe.
 
-				timespec ts{};
-				ts.tv_sec = epoch;
-				ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
+                timespec ts{};
+                ts.tv_sec  = epoch;
+                ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
 
-				setClock(ts);
+                setClock(ts);
 
-				_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
-				_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+                _gps_position->time_utc_usec  = static_cast<uint64_t>(epoch) * 1000000ULL;
+                _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
 
-			} else {
-				_gps_position->time_utc_usec = 0;
-			}
+            } else {
+                _gps_position->time_utc_usec = 0;
+            }
 
 #else
-			_gps_position->time_utc_usec = 0;
+            _gps_position->time_utc_usec = 0;
 #endif
-		}
+        }
 
-		_last_timestamp_time = gps_absolute_time();
+        _last_timestamp_time = gps_absolute_time();
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_SAT:
-	case UBX_MSG_NAV_SVINFO:
-		UBX_TRACE_RXMSG("Rx NAV-SVINFO");
+    case UBX_MSG_NAV_SAT:
+    case UBX_MSG_NAV_SVINFO:
+        UBX_TRACE_RXMSG("Rx NAV-SVINFO");
 
-		// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
-		_satellite_info->timestamp = gps_absolute_time();
+        // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
+        _satellite_info->timestamp = gps_absolute_time();
 
-		ret = 2;
-		break;
+        ret = 2;
+        break;
 
-	case UBX_MSG_NAV_SVIN:
-		UBX_TRACE_RXMSG("Rx NAV-SVIN");
-		{
-			ubx_payload_rx_nav_svin_t &svin = _buf.payload_rx_nav_svin;
+    case UBX_MSG_NAV_SVIN:
+        UBX_TRACE_RXMSG("Rx NAV-SVIN");
+        {
+            ubx_payload_rx_nav_svin_t &svin = _buf.payload_rx_nav_svin;
 
-			UBX_DEBUG("Survey-in status: %lus cur accuracy: %lumm nr obs: %lu valid: %i active: %i",
-				  svin.dur, svin.meanAcc / 10, svin.obs, static_cast<int>(svin.valid), static_cast<int>(svin.active));
+            UBX_DEBUG("Survey-in status: %lus cur accuracy: %lumm nr obs: %lu valid: %i active: %i",
+                      svin.dur, svin.meanAcc / 10, svin.obs, static_cast<int>(svin.valid), static_cast<int>(svin.active));
 
-			SurveyInStatus status{};
-			double ecef_x = (static_cast<double>(svin.meanX) + static_cast<double>(svin.meanXHP) * 0.01) * 0.01;
-			double ecef_y = (static_cast<double>(svin.meanY) + static_cast<double>(svin.meanYHP) * 0.01) * 0.01;
-			double ecef_z = (static_cast<double>(svin.meanZ) + static_cast<double>(svin.meanZHP) * 0.01) * 0.01;
-			ECEF2lla(ecef_x, ecef_y, ecef_z, status.latitude, status.longitude, status.altitude);
-			status.duration = svin.dur;
-			status.mean_accuracy = svin.meanAcc / 10;
-			status.flags = (svin.valid & 1) | ((svin.active & 1) << 1);
-			surveyInStatus(status);
+            SurveyInStatus status{};
+            double         ecef_x = (static_cast<double>(svin.meanX) + static_cast<double>(svin.meanXHP) * 0.01) * 0.01;
+            double         ecef_y = (static_cast<double>(svin.meanY) + static_cast<double>(svin.meanYHP) * 0.01) * 0.01;
+            double         ecef_z = (static_cast<double>(svin.meanZ) + static_cast<double>(svin.meanZHP) * 0.01) * 0.01;
+            ECEF2lla(ecef_x, ecef_y, ecef_z, status.latitude, status.longitude, status.altitude);
+            status.duration      = svin.dur;
+            status.mean_accuracy = svin.meanAcc / 10;
+            status.flags         = (svin.valid & 1) | ((svin.active & 1) << 1);
+            surveyInStatus(status);
 
-			if (svin.valid == 1 && svin.active == 0) {
-				if (activateRTCMOutput(true) != 0) {
-					return 0;
-				}
-			}
-		}
+            if (svin.valid == 1 && svin.active == 0) {
+                if (activateRTCMOutput(true) != 0) {
+                    return 0;
+                }
+            }
+        }
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_VELNED:
-		UBX_TRACE_RXMSG("Rx NAV-VELNED");
+    case UBX_MSG_NAV_VELNED:
+        UBX_TRACE_RXMSG("Rx NAV-VELNED");
 
-		_gps_position->vel_m_s        = static_cast<float>(_buf.payload_rx_nav_velned.gSpeed) * 1e-2f;
-		_gps_position->vel_n_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velN)  * 1e-2f; // NED NORTH velocity
-		_gps_position->vel_e_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velE)  * 1e-2f; // NED EAST velocity
-		_gps_position->vel_d_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velD)  * 1e-2f; // NED DOWN velocity
-		_gps_position->cog_rad        = static_cast<float>(_buf.payload_rx_nav_velned.heading) * M_DEG_TO_RAD_F * 1e-5f;
-		_gps_position->c_variance_rad = static_cast<float>(_buf.payload_rx_nav_velned.cAcc)    * M_DEG_TO_RAD_F * 1e-5f;
-		_gps_position->vel_ned_valid  = true;
+        _gps_position->vel_m_s        = static_cast<float>(_buf.payload_rx_nav_velned.gSpeed) * 1e-2f;
+        _gps_position->vel_n_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velN) * 1e-2f; // NED NORTH velocity
+        _gps_position->vel_e_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velE) * 1e-2f; // NED EAST velocity
+        _gps_position->vel_d_m_s      = static_cast<float>(_buf.payload_rx_nav_velned.velD) * 1e-2f; // NED DOWN velocity
+        _gps_position->cog_rad        = static_cast<float>(_buf.payload_rx_nav_velned.heading) * M_DEG_TO_RAD_F * 1e-5f;
+        _gps_position->c_variance_rad = static_cast<float>(_buf.payload_rx_nav_velned.cAcc) * M_DEG_TO_RAD_F * 1e-5f;
+        _gps_position->vel_ned_valid  = true;
 
-		_rate_count_vel++;
-		_got_velned = true;
+        _rate_count_vel++;
+        _got_velned = true;
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_NAV_RELPOSNED:
-		UBX_TRACE_RXMSG("Rx NAV-RELPOSNED");
+    case UBX_MSG_NAV_RELPOSNED:
+        UBX_TRACE_RXMSG("Rx NAV-RELPOSNED");
 
-		if ((_mode == UBXMode::RoverWithMovingBase) || (_mode == UBXMode::RoverWithMovingBaseUART1)) {
-			float heading = _buf.payload_rx_nav_relposned.relPosHeading * 1e-5f;
-			float heading_acc = _buf.payload_rx_nav_relposned.accHeading * 1e-5f;
-			float rel_length = _buf.payload_rx_nav_relposned.relPosLength + _buf.payload_rx_nav_relposned.relPosHPLength * 1e-2f;
-			float rel_length_acc = _buf.payload_rx_nav_relposned.accLength * 1e-2f;
-			bool heading_valid = _buf.payload_rx_nav_relposned.flags & (1 << 8);
-			bool rel_pos_valid = _buf.payload_rx_nav_relposned.flags & (1 << 2);
-			bool carrier_solution_fixed = _buf.payload_rx_nav_relposned.flags & (1 << 4);
-			(void)rel_length_acc;
+        if ((_mode == UBXMode::RoverWithMovingBase) || (_mode == UBXMode::RoverWithMovingBaseUART1)) {
+            float heading                = _buf.payload_rx_nav_relposned.relPosHeading * 1e-5f;
+            float heading_acc            = _buf.payload_rx_nav_relposned.accHeading * 1e-5f;
+            float rel_length             = _buf.payload_rx_nav_relposned.relPosLength + _buf.payload_rx_nav_relposned.relPosHPLength * 1e-2f;
+            float rel_length_acc         = _buf.payload_rx_nav_relposned.accLength * 1e-2f;
+            bool  heading_valid          = _buf.payload_rx_nav_relposned.flags & (1 << 8);
+            bool  rel_pos_valid          = _buf.payload_rx_nav_relposned.flags & (1 << 2);
+            bool  carrier_solution_fixed = _buf.payload_rx_nav_relposned.flags & (1 << 4);
+            (void)rel_length_acc;
 
-			if (heading_valid && rel_pos_valid && rel_length < 1000.f && carrier_solution_fixed) { // validity & sanity checks
-				heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
-				heading -= _heading_offset; // range: [-pi, 3pi]
+            if (heading_valid && rel_pos_valid && rel_length < 1000.f && carrier_solution_fixed) { // validity & sanity checks
+                heading *= M_PI_F / 180.0f;                                                        // deg to rad, now in range [0, 2pi]
+                heading -= _heading_offset;                                                        // range: [-pi, 3pi]
 
-				if (heading > M_PI_F) {
-					heading -= 2.f * M_PI_F; // final range is [-pi, pi]
-				}
+                if (heading > M_PI_F) {
+                    heading -= 2.f * M_PI_F; // final range is [-pi, pi]
+                }
 
-				_gps_position->heading = heading;
+                _gps_position->heading = heading;
 
-				heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
+                heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
 
-				_gps_position->heading_accuracy = heading_acc;
+                _gps_position->heading_accuracy = heading_acc;
 
-				UBX_DEBUG("Heading: %.3f rad, acc: %.1f deg, relLen: %.1f cm, relAcc: %.1f cm, valid: %i %i", (double)heading,
-					  (double)heading_acc, (double)rel_length, (double)rel_length_acc, heading_valid, rel_pos_valid);
-			}
+                UBX_DEBUG("Heading: %.3f rad, acc: %.1f deg, relLen: %.1f cm, relAcc: %.1f cm, valid: %i %i", (double)heading,
+                          (double)heading_acc, (double)rel_length, (double)rel_length_acc, heading_valid, rel_pos_valid);
+            }
 
-			ret = 1;
-		}
+            ret = 1;
+        }
 
-		{
-			sensor_gnss_relative_s gps_rel{};
+        {
+            sensor_gnss_relative_s gps_rel{};
 
-			gps_rel.timestamp_sample = gps_absolute_time(); // TODO: adjust with delay estimate
+            gps_rel.timestamp_sample = gps_absolute_time();                           // TODO: adjust with delay estimate
 
-			gps_rel.time_utc_usec = _buf.payload_rx_nav_relposned.iTOW * 1000; // TODO: convert iTOW ms GPS time of week
-			gps_rel.reference_station_id = _buf.payload_rx_nav_relposned.refStationId;
+            gps_rel.time_utc_usec        = _buf.payload_rx_nav_relposned.iTOW * 1000; // TODO: convert iTOW ms GPS time of week
+            gps_rel.reference_station_id = _buf.payload_rx_nav_relposned.refStationId;
 
-			// relPosN + (relPosHPN * 1e-2), relPosHPN is 0.1 mm
-			gps_rel.position[0] = (_buf.payload_rx_nav_relposned.relPosN + _buf.payload_rx_nav_relposned.relPosHPN * 1e-2f) * 1e-2f;
-			gps_rel.position[1] = (_buf.payload_rx_nav_relposned.relPosE + _buf.payload_rx_nav_relposned.relPosHPE * 1e-2f) * 1e-2f;
-			gps_rel.position[2] = (_buf.payload_rx_nav_relposned.relPosD + _buf.payload_rx_nav_relposned.relPosHPD * 1e-2f) * 1e-2f;
+            // relPosN + (relPosHPN * 1e-2), relPosHPN is 0.1 mm
+            gps_rel.position[0] = (_buf.payload_rx_nav_relposned.relPosN + _buf.payload_rx_nav_relposned.relPosHPN * 1e-2f) * 1e-2f;
+            gps_rel.position[1] = (_buf.payload_rx_nav_relposned.relPosE + _buf.payload_rx_nav_relposned.relPosHPE * 1e-2f) * 1e-2f;
+            gps_rel.position[2] = (_buf.payload_rx_nav_relposned.relPosD + _buf.payload_rx_nav_relposned.relPosHPD * 1e-2f) * 1e-2f;
 
-			// full length of the relative position vector, in units of cm, is given by relPosLength + (relPosHPLength * 1e-2)
-			gps_rel.position_length = (_buf.payload_rx_nav_relposned.relPosLength
-						   + _buf.payload_rx_nav_relposned.relPosHPLength * 1e-2f) * 1e-2f;
+            // full length of the relative position vector, in units of cm, is given by relPosLength + (relPosHPLength * 1e-2)
+            gps_rel.position_length = (_buf.payload_rx_nav_relposned.relPosLength
+                                       + _buf.payload_rx_nav_relposned.relPosHPLength * 1e-2f)
+                                    * 1e-2f;
 
-			gps_rel.heading = _buf.payload_rx_nav_relposned.relPosHeading * 1e-5f * (M_PI_F / 180.f);  // 1e-5 deg -> radians
-			gps_rel.heading_accuracy = _buf.payload_rx_nav_relposned.accHeading * 1e-5f * (M_PI_F / 180.f); // 1e-5 deg -> radians
+            gps_rel.heading          = _buf.payload_rx_nav_relposned.relPosHeading * 1e-5f * (M_PI_F / 180.f); // 1e-5 deg -> radians
+            gps_rel.heading_accuracy = _buf.payload_rx_nav_relposned.accHeading * 1e-5f * (M_PI_F / 180.f);    // 1e-5 deg -> radians
 
-			// Accuracy of relative position in 0.1 mm
-			gps_rel.position_accuracy[0] = _buf.payload_rx_nav_relposned.accN * 1e-4f; // 0.1mm -> m
-			gps_rel.position_accuracy[1] = _buf.payload_rx_nav_relposned.accE * 1e-4f; // 0.1mm -> m
-			gps_rel.position_accuracy[2] = _buf.payload_rx_nav_relposned.accD * 1e-4f; // 0.1mm -> m
+            // Accuracy of relative position in 0.1 mm
+            gps_rel.position_accuracy[0] = _buf.payload_rx_nav_relposned.accN * 1e-4f; // 0.1mm -> m
+            gps_rel.position_accuracy[1] = _buf.payload_rx_nav_relposned.accE * 1e-4f; // 0.1mm -> m
+            gps_rel.position_accuracy[2] = _buf.payload_rx_nav_relposned.accD * 1e-4f; // 0.1mm -> m
 
-			gps_rel.accuracy_length = _buf.payload_rx_nav_relposned.accLength * 1e-4f; // 0.1mm -> m
+            gps_rel.accuracy_length = _buf.payload_rx_nav_relposned.accLength * 1e-4f; // 0.1mm -> m
 
-			gps_rel.gnss_fix_ok                  = _buf.payload_rx_nav_relposned.flags & (1 << 0);
-			gps_rel.differential_solution        = _buf.payload_rx_nav_relposned.flags & (1 << 1);
-			gps_rel.relative_position_valid      = _buf.payload_rx_nav_relposned.flags & (1 << 2);
-			gps_rel.carrier_solution_floating    = _buf.payload_rx_nav_relposned.flags & (1 << 3);
-			gps_rel.carrier_solution_fixed       = _buf.payload_rx_nav_relposned.flags & (1 << 4);
-			gps_rel.moving_base_mode             = _buf.payload_rx_nav_relposned.flags & (1 << 5);
-			gps_rel.reference_position_miss      = _buf.payload_rx_nav_relposned.flags & (1 << 6);
-			gps_rel.reference_observations_miss  = _buf.payload_rx_nav_relposned.flags & (1 << 7);
-			gps_rel.heading_valid                = _buf.payload_rx_nav_relposned.flags & (1 << 8);
-			gps_rel.relative_position_normalized = _buf.payload_rx_nav_relposned.flags & (1 << 9);
+            gps_rel.gnss_fix_ok                  = _buf.payload_rx_nav_relposned.flags & (1 << 0);
+            gps_rel.differential_solution        = _buf.payload_rx_nav_relposned.flags & (1 << 1);
+            gps_rel.relative_position_valid      = _buf.payload_rx_nav_relposned.flags & (1 << 2);
+            gps_rel.carrier_solution_floating    = _buf.payload_rx_nav_relposned.flags & (1 << 3);
+            gps_rel.carrier_solution_fixed       = _buf.payload_rx_nav_relposned.flags & (1 << 4);
+            gps_rel.moving_base_mode             = _buf.payload_rx_nav_relposned.flags & (1 << 5);
+            gps_rel.reference_position_miss      = _buf.payload_rx_nav_relposned.flags & (1 << 6);
+            gps_rel.reference_observations_miss  = _buf.payload_rx_nav_relposned.flags & (1 << 7);
+            gps_rel.heading_valid                = _buf.payload_rx_nav_relposned.flags & (1 << 8);
+            gps_rel.relative_position_normalized = _buf.payload_rx_nav_relposned.flags & (1 << 9);
 
-			gotRelativePositionMessage(gps_rel);
-		}
+            gotRelativePositionMessage(gps_rel);
+        }
 
-		break;
+        break;
 
-	case UBX_MSG_MON_VER:
-		UBX_TRACE_RXMSG("Rx MON-VER");
+    case UBX_MSG_MON_VER:
+        UBX_TRACE_RXMSG("Rx MON-VER");
 
-		// This is polled only on startup, and the startup code waits for an ack
-		if (_ack_state == UBX_ACK_WAITING && _ack_waiting_msg == UBX_MSG_MON_VER) {
-			_ack_state = UBX_ACK_GOT_ACK;
-		}
+        // This is polled only on startup, and the startup code waits for an ack
+        if (_ack_state == UBX_ACK_WAITING && _ack_waiting_msg == UBX_MSG_MON_VER) {
+            _ack_state = UBX_ACK_GOT_ACK;
+        }
 
-		ret = 1;
-		break;
+        ret = 1;
+        break;
 
-	case UBX_MSG_MON_HW:
-		UBX_TRACE_RXMSG("Rx MON-HW");
+    case UBX_MSG_MON_HW:
+        UBX_TRACE_RXMSG("Rx MON-HW");
 
-		switch (_rx_payload_length) {
+        switch (_rx_payload_length) {
+        case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */
+            _gps_position->noise_per_ms           = _buf.payload_rx_mon_hw_ubx6.noisePerMS;
+            _gps_position->automatic_gain_control = _buf.payload_rx_mon_hw_ubx6.agcCnt;
+            _gps_position->jamming_indicator      = _buf.payload_rx_mon_hw_ubx6.jamInd;
 
-		case sizeof(ubx_payload_rx_mon_hw_ubx6_t):	/* u-blox 6 msg format */
-			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx6.noisePerMS;
-			_gps_position->automatic_gain_control   = _buf.payload_rx_mon_hw_ubx6.agcCnt;
-			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx6.jamInd;
+            ret = 1;
+            break;
 
-			ret = 1;
-			break;
+        case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */
+            _gps_position->noise_per_ms           = _buf.payload_rx_mon_hw_ubx7.noisePerMS;
+            _gps_position->automatic_gain_control = _buf.payload_rx_mon_hw_ubx7.agcCnt;
+            _gps_position->jamming_indicator      = _buf.payload_rx_mon_hw_ubx7.jamInd;
 
-		case sizeof(ubx_payload_rx_mon_hw_ubx7_t):	/* u-blox 7+ msg format */
-			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx7.noisePerMS;
-			_gps_position->automatic_gain_control   = _buf.payload_rx_mon_hw_ubx7.agcCnt;
-			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx7.jamInd;
+            ret = 1;
+            break;
 
-			ret = 1;
-			break;
+        default:     // unexpected payload size:
+            ret = 0; // don't handle message
+            break;
+        }
 
-		default:		// unexpected payload size:
-			ret = 0;	// don't handle message
-			break;
-		}
+        break;
 
-		break;
+    case UBX_MSG_MON_RF:
+        UBX_TRACE_RXMSG("Rx MON-RF");
 
-	case UBX_MSG_MON_RF:
-		UBX_TRACE_RXMSG("Rx MON-RF");
+        _gps_position->noise_per_ms      = _buf.payload_rx_mon_rf.block[0].noisePerMS;
+        _gps_position->jamming_indicator = _buf.payload_rx_mon_rf.block[0].jamInd;
+        _gps_position->jamming_state     = _buf.payload_rx_mon_rf.block[0].flags;
 
-		_gps_position->noise_per_ms		= _buf.payload_rx_mon_rf.block[0].noisePerMS;
-		_gps_position->jamming_indicator	= _buf.payload_rx_mon_rf.block[0].jamInd;
-		_gps_position->jamming_state		= _buf.payload_rx_mon_rf.block[0].flags;
+        ret = 1;
+        break;
 
-		ret = 1;
-		break;
+    case UBX_MSG_ACK_ACK:
+        UBX_TRACE_RXMSG("Rx ACK-ACK");
 
-	case UBX_MSG_ACK_ACK:
-		UBX_TRACE_RXMSG("Rx ACK-ACK");
+        if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+            _ack_state = UBX_ACK_GOT_ACK;
+        }
 
-		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
-			_ack_state = UBX_ACK_GOT_ACK;
-		}
+        ret = 1;
+        break;
 
-		ret = 1;
-		break;
+    case UBX_MSG_ACK_NAK:
+        UBX_TRACE_RXMSG("Rx ACK-NAK");
 
-	case UBX_MSG_ACK_NAK:
-		UBX_TRACE_RXMSG("Rx ACK-NAK");
+        if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+            _ack_state = UBX_ACK_GOT_NAK;
+        }
 
-		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
-			_ack_state = UBX_ACK_GOT_NAK;
-		}
+        ret = 1;
+        break;
 
-		ret = 1;
-		break;
+    default:
+        break;
+    }
 
-	default:
-		break;
-	}
+    if (ret > 0) {
+        _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
+    }
 
-	if (ret > 0) {
-		_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
-	}
-
-	return ret;
+    return ret;
 }
 
-int
-GPSDriverUBX::activateRTCMOutput(bool reduce_update_rate)
-{
-	/* For base stations we switch to 1 Hz update rate, which is enough for RTCM output.
+int GPSDriverUBX::activateRTCMOutput(bool reduce_update_rate) {
+    /* For base stations we switch to 1 Hz update rate, which is enough for RTCM output.
 	 * For the survey-in, we still want 5/10 Hz, because this speeds up the process */
 
-	if (_proto_ver_27_or_higher) {
-		int cfg_valset_msg_size = initCfgValset();
-
-		if (reduce_update_rate) {
-			cfgValset<uint16_t>(UBX_CFG_KEY_RATE_MEAS, 1000, cfg_valset_msg_size);
-		}
-
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, 5, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, 1, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, 1, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, 1, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, 1, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, 1, cfg_valset_msg_size);
-		cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, 0, cfg_valset_msg_size);
-
-		if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
-			return -1;
-		}
-
-		if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false) < 0) {
-			return -1;
-		}
-
-	} else {
-
-		if (reduce_update_rate) {
-			memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
-			_buf.payload_tx_cfg_rate.measRate	= 1000;
-			_buf.payload_tx_cfg_rate.navRate	= UBX_TX_CFG_RATE_NAVRATE;
-			_buf.payload_tx_cfg_rate.timeRef	= UBX_TX_CFG_RATE_TIMEREF;
-
-			if (!sendMessage(UBX_MSG_CFG_RATE, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rate))) { return -1; }
-
-			// according to the spec we should receive an (N)ACK here, but we don't
-		}
-
-		configureMessageRate(UBX_MSG_NAV_SVIN, 0);
-
-		// stationary RTK reference station ARP (can be sent at lower rate)
-		if (!configureMessageRate(UBX_MSG_RTCM3_1005, 5)) { return -1; }
-
-		// GPS
-		if (!configureMessageRate(UBX_MSG_RTCM3_1077, 1)) { return -1; }
-
-		// GLONASS
-		if (!configureMessageRate(UBX_MSG_RTCM3_1087, 1)) { return -1; }
-
-		// GLONASS code-phase biases
-		if (!configureMessageRate(UBX_MSG_RTCM3_1230, 1)) { return -1; }
-
-		// Galileo
-		if (!configureMessageRate(UBX_MSG_RTCM3_1097, 1)) { return -1; }
-
-		// BeiDou
-		if (!configureMessageRate(UBX_MSG_RTCM3_1127, 1)) { return -1; }
-	}
-
-	return 0;
+    if (_proto_ver_27_or_higher) {
+        int cfg_valset_msg_size = initCfgValset();
+
+        if (reduce_update_rate) {
+            cfgValset<uint16_t>(UBX_CFG_KEY_RATE_MEAS, 1000, cfg_valset_msg_size);
+        }
+
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C, 5, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C, 1, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C, 1, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C, 1, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C, 1, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C, 1, cfg_valset_msg_size);
+        cfgValsetPort(UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C, 0, cfg_valset_msg_size);
+
+        if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
+            return -1;
+        }
+
+        if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, false) < 0) {
+            return -1;
+        }
+
+    } else {
+        if (reduce_update_rate) {
+            memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
+            _buf.payload_tx_cfg_rate.measRate = 1000;
+            _buf.payload_tx_cfg_rate.navRate  = UBX_TX_CFG_RATE_NAVRATE;
+            _buf.payload_tx_cfg_rate.timeRef  = UBX_TX_CFG_RATE_TIMEREF;
+
+            if (!sendMessage(UBX_MSG_CFG_RATE, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rate))) {
+                return -1;
+            }
+
+            // according to the spec we should receive an (N)ACK here, but we don't
+        }
+
+        configureMessageRate(UBX_MSG_NAV_SVIN, 0);
+
+        // stationary RTK reference station ARP (can be sent at lower rate)
+        if (!configureMessageRate(UBX_MSG_RTCM3_1005, 5)) {
+            return -1;
+        }
+
+        // GPS
+        if (!configureMessageRate(UBX_MSG_RTCM3_1077, 1)) {
+            return -1;
+        }
+
+        // GLONASS
+        if (!configureMessageRate(UBX_MSG_RTCM3_1087, 1)) {
+            return -1;
+        }
+
+        // GLONASS code-phase biases
+        if (!configureMessageRate(UBX_MSG_RTCM3_1230, 1)) {
+            return -1;
+        }
+
+        // Galileo
+        if (!configureMessageRate(UBX_MSG_RTCM3_1097, 1)) {
+            return -1;
+        }
+
+        // BeiDou
+        if (!configureMessageRate(UBX_MSG_RTCM3_1127, 1)) {
+            return -1;
+        }
+    }
+
+    return 0;
 }
 
-void
-GPSDriverUBX::decodeInit()
-{
-	_decode_state = UBX_DECODE_SYNC1;
-	_rx_ck_a = 0;
-	_rx_ck_b = 0;
-	_rx_payload_length = 0;
-	_rx_payload_index = 0;
-
-	if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM || _mode == UBXMode::MovingBaseUART1) {
-		if (!_rtcm_parsing) {
-			_rtcm_parsing = new RTCMParsing();
-		}
-
-		if (_rtcm_parsing) {
-			_rtcm_parsing->reset();
-		}
-	}
+void GPSDriverUBX::decodeInit() {
+    _decode_state      = UBX_DECODE_SYNC1;
+    _rx_ck_a           = 0;
+    _rx_ck_b           = 0;
+    _rx_payload_length = 0;
+    _rx_payload_index  = 0;
+
+    if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM || _mode == UBXMode::MovingBaseUART1) {
+        if (!_rtcm_parsing) {
+            _rtcm_parsing = new RTCMParsing();
+        }
+
+        if (_rtcm_parsing) {
+            _rtcm_parsing->reset();
+        }
+    }
 }
 
-void
-GPSDriverUBX::addByteToChecksum(const uint8_t b)
-{
-	_rx_ck_a = _rx_ck_a + b;
-	_rx_ck_b = _rx_ck_b + _rx_ck_a;
+void GPSDriverUBX::addByteToChecksum(const uint8_t b) {
+    _rx_ck_a = _rx_ck_a + b;
+    _rx_ck_b = _rx_ck_b + _rx_ck_a;
 }
 
-void
-GPSDriverUBX::calcChecksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum)
-{
-	for (uint16_t i = 0; i < length; i++) {
-		checksum->ck_a = checksum->ck_a + buffer[i];
-		checksum->ck_b = checksum->ck_b + checksum->ck_a;
-	}
+void GPSDriverUBX::calcChecksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum) {
+    for (uint16_t i = 0; i < length; i++) {
+        checksum->ck_a = checksum->ck_a + buffer[i];
+        checksum->ck_b = checksum->ck_b + checksum->ck_a;
+    }
 }
 
-bool
-GPSDriverUBX::configureMessageRate(const uint16_t msg, const uint8_t rate)
-{
-	if (_proto_ver_27_or_higher) {
-		// configureMessageRate() should not be called if _proto_ver_27_or_higher is true.
-		// If you see this message the calling code needs to be fixed.
-		UBX_WARN("FIXME: use of deprecated msg CFG_MSG (%i %i)", msg, rate);
-	}
+bool GPSDriverUBX::configureMessageRate(const uint16_t msg, const uint8_t rate) {
+    if (_proto_ver_27_or_higher) {
+        // configureMessageRate() should not be called if _proto_ver_27_or_higher is true.
+        // If you see this message the calling code needs to be fixed.
+        UBX_WARN("FIXME: use of deprecated msg CFG_MSG (%i %i)", msg, rate);
+    }
 
-	ubx_payload_tx_cfg_msg_t cfg_msg;	// don't use _buf (allow interleaved operation)
-	memset(&cfg_msg, 0, sizeof(cfg_msg));
+    ubx_payload_tx_cfg_msg_t cfg_msg; // don't use _buf (allow interleaved operation)
+    memset(&cfg_msg, 0, sizeof(cfg_msg));
 
-	cfg_msg.msg	= msg;
-	cfg_msg.rate	= rate;
+    cfg_msg.msg  = msg;
+    cfg_msg.rate = rate;
 
-	return sendMessage(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg));
+    return sendMessage(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg));
 }
 
-bool
-GPSDriverUBX::configureMessageRateAndAck(uint16_t msg, uint8_t rate, bool report_ack_error)
-{
-	if (!configureMessageRate(msg, rate)) {
-		return false;
-	}
+bool GPSDriverUBX::configureMessageRateAndAck(uint16_t msg, uint8_t rate, bool report_ack_error) {
+    if (!configureMessageRate(msg, rate)) {
+        return false;
+    }
 
-	return waitForAck(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, report_ack_error) >= 0;
+    return waitForAck(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, report_ack_error) >= 0;
 }
 
-bool
-GPSDriverUBX::sendMessage(const uint16_t msg, const uint8_t *payload, const uint16_t length)
-{
-	ubx_header_t   header = {UBX_SYNC1, UBX_SYNC2, 0, 0};
-	ubx_checksum_t checksum = {0, 0};
+bool GPSDriverUBX::sendMessage(const uint16_t msg, const uint8_t *payload, const uint16_t length) {
+    ubx_header_t   header   = {UBX_SYNC1, UBX_SYNC2, 0, 0};
+    ubx_checksum_t checksum = {0, 0};
 
-	// Populate header
-	header.msg	= msg;
-	header.length	= length;
+    // Populate header
+    header.msg    = msg;
+    header.length = length;
 
-	// Calculate checksum
-	calcChecksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
+    // Calculate checksum
+    calcChecksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
 
-	if (payload != nullptr) {
-		calcChecksum(payload, length, &checksum);
-	}
+    if (payload != nullptr) {
+        calcChecksum(payload, length, &checksum);
+    }
 
-	// Send message
-	if (write((void *)&header, sizeof(header)) != sizeof(header)) {
-		return false;
-	}
+    // Send message
+    if (write((void *)&header, sizeof(header)) != sizeof(header)) {
+        return false;
+    }
 
-	if (payload && write((void *)payload, length) != length) {
-		return false;
-	}
+    if (payload && write((void *)payload, length) != length) {
+        return false;
+    }
 
-	if (write((void *)&checksum, sizeof(checksum)) != sizeof(checksum)) {
-		return false;
-	}
+    if (write((void *)&checksum, sizeof(checksum)) != sizeof(checksum)) {
+        return false;
+    }
 
-	return true;
+    return true;
 }
 
 uint32_t
-GPSDriverUBX::fnv1_32_str(uint8_t *str, uint32_t hval)
-{
-	uint8_t *s = str;
+GPSDriverUBX::fnv1_32_str(uint8_t *str, uint32_t hval) {
+    uint8_t *s = str;
 
-	/*
+    /*
 	 * FNV-1 hash each octet in the buffer
 	 */
-	while (*s) {
-
-		/* multiply by the 32 bit FNV magic prime mod 2^32 */
+    while (*s) {
+        /* multiply by the 32 bit FNV magic prime mod 2^32 */
 #if defined(NO_FNV_GCC_OPTIMIZATION)
-		hval *= FNV1_32_PRIME;
+        hval *= FNV1_32_PRIME;
 #else
-		hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24);
+        hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24);
 #endif
 
-		/* xor the bottom with the current octet */
-		hval ^= (uint32_t) * s++;
-	}
+        /* xor the bottom with the current octet */
+        hval ^= (uint32_t)*s++;
+    }
 
-	/* return our new hash value */
-	return hval;
+    /* return our new hash value */
+    return hval;
 }
 
-int
-GPSDriverUBX::reset(GPSRestartType restart_type)
-{
-	memset(&_buf.payload_tx_cfg_rst, 0, sizeof(_buf.payload_tx_cfg_rst));
-	_buf.payload_tx_cfg_rst.resetMode = UBX_TX_CFG_RST_MODE_SOFTWARE;
+int GPSDriverUBX::reset(GPSRestartType restart_type) {
+    memset(&_buf.payload_tx_cfg_rst, 0, sizeof(_buf.payload_tx_cfg_rst));
+    _buf.payload_tx_cfg_rst.resetMode = UBX_TX_CFG_RST_MODE_SOFTWARE;
 
-	switch (restart_type) {
-	case GPSRestartType::Hot:
-		_buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_HOT_START;
-		break;
+    switch (restart_type) {
+    case GPSRestartType::Hot:
+        _buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_HOT_START;
+        break;
 
-	case GPSRestartType::Warm:
-		_buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_WARM_START;
-		break;
+    case GPSRestartType::Warm:
+        _buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_WARM_START;
+        break;
 
-	case GPSRestartType::Cold:
-		_buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_COLD_START;
-		break;
+    case GPSRestartType::Cold:
+        _buf.payload_tx_cfg_rst.navBbrMask = UBX_TX_CFG_RST_BBR_MODE_COLD_START;
+        break;
 
-	default:
-		return -2;
-	}
+    default:
+        return -2;
+    }
 
-	if (sendMessage(UBX_MSG_CFG_RST, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rst))) {
-		return 0;
-	}
+    if (sendMessage(UBX_MSG_CFG_RST, (uint8_t *)&_buf, sizeof(_buf.payload_tx_cfg_rst))) {
+        return 0;
+    }
 
-	return -2;
+    return -2;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/ubx.h b/apps/peripheral/sensor/gnss/gps/devices/src/ubx.h
index a1acecbd85..1671359a8c 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/ubx.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/ubx.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2012-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ubx.h
@@ -54,78 +31,78 @@
 #include "../../definitions.h"
 
 
-#define UBX_CONFIG_TIMEOUT    250 // ms, timeout for waiting ACK
-#define UBX_PACKET_TIMEOUT    8   // ms, if now data during this delay assume that full update received
+#define UBX_CONFIG_TIMEOUT     250                               // ms, timeout for waiting ACK
+#define UBX_PACKET_TIMEOUT     8                                 // ms, if now data during this delay assume that full update received
 
-#define DISABLE_MSG_INTERVAL  1000000    // us, try to disable message with this interval
+#define DISABLE_MSG_INTERVAL   1000000                           // us, try to disable message with this interval
 
-#define FNV1_32_INIT          static_cast<uint32_t>(0x811c9dc5)    // init value for FNV1 hash algorithm
-#define FNV1_32_PRIME         static_cast<uint32_t>(0x01000193)    // magic prime for FNV1 hash algorithm
+#define FNV1_32_INIT           static_cast<uint32_t>(0x811c9dc5) // init value for FNV1 hash algorithm
+#define FNV1_32_PRIME          static_cast<uint32_t>(0x01000193) // magic prime for FNV1 hash algorithm
 
-#define UBX_SYNC1             0xB5
-#define UBX_SYNC2             0x62
+#define UBX_SYNC1              0xB5
+#define UBX_SYNC2              0x62
 
 #define UART1_BAUDRATE_HEADING 921600
 
 /* Message Classes */
-#define UBX_CLASS_NAV         0x01
-#define UBX_CLASS_RXM         0x02
-#define UBX_CLASS_INF         0x04
-#define UBX_CLASS_ACK         0x05
-#define UBX_CLASS_CFG         0x06
-#define UBX_CLASS_MON         0x0A
-#define UBX_CLASS_RTCM3       0xF5
+#define UBX_CLASS_NAV   0x01
+#define UBX_CLASS_RXM   0x02
+#define UBX_CLASS_INF   0x04
+#define UBX_CLASS_ACK   0x05
+#define UBX_CLASS_CFG   0x06
+#define UBX_CLASS_MON   0x0A
+#define UBX_CLASS_RTCM3 0xF5
 
 /* Message IDs */
-#define UBX_ID_NAV_POSLLH     0x02
-#define UBX_ID_NAV_DOP        0x04
-#define UBX_ID_NAV_SOL        0x06
-#define UBX_ID_NAV_PVT        0x07
-#define UBX_ID_NAV_VELNED     0x12
-#define UBX_ID_NAV_TIMEUTC    0x21
-#define UBX_ID_NAV_SVINFO     0x30
-#define UBX_ID_NAV_SAT        0x35
-#define UBX_ID_NAV_STATUS     0x03
-#define UBX_ID_NAV_SVIN       0x3B
-#define UBX_ID_NAV_RELPOSNED  0x3C
-#define UBX_ID_RXM_SFRBX      0x13
-#define UBX_ID_RXM_RAWX       0x15
-#define UBX_ID_INF_DEBUG      0x04
-#define UBX_ID_INF_ERROR      0x00
-#define UBX_ID_INF_NOTICE     0x02
-#define UBX_ID_INF_WARNING    0x01
-#define UBX_ID_ACK_NAK        0x00
-#define UBX_ID_ACK_ACK        0x01
-#define UBX_ID_CFG_PRT        0x00 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_MSG        0x01 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_RATE       0x08 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_CFG        0x09 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_NAV5       0x24 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_RST        0x04
-#define UBX_ID_CFG_SBAS       0x16
-#define UBX_ID_CFG_TMODE3     0x71 // deprecated in protocol version >= 27 -> use CFG_VALSET
-#define UBX_ID_CFG_GNSS       0x3E
-#define UBX_ID_CFG_VALSET     0x8A
-#define UBX_ID_CFG_VALGET     0x8B
-#define UBX_ID_CFG_VALDEL     0x8C
-#define UBX_ID_MON_VER        0x04
-#define UBX_ID_MON_HW         0x09 // deprecated in protocol version >= 27 -> use MON_RF
-#define UBX_ID_MON_RF         0x38
+#define UBX_ID_NAV_POSLLH    0x02
+#define UBX_ID_NAV_DOP       0x04
+#define UBX_ID_NAV_SOL       0x06
+#define UBX_ID_NAV_PVT       0x07
+#define UBX_ID_NAV_VELNED    0x12
+#define UBX_ID_NAV_TIMEUTC   0x21
+#define UBX_ID_NAV_SVINFO    0x30
+#define UBX_ID_NAV_SAT       0x35
+#define UBX_ID_NAV_STATUS    0x03
+#define UBX_ID_NAV_SVIN      0x3B
+#define UBX_ID_NAV_RELPOSNED 0x3C
+#define UBX_ID_RXM_SFRBX     0x13
+#define UBX_ID_RXM_RAWX      0x15
+#define UBX_ID_INF_DEBUG     0x04
+#define UBX_ID_INF_ERROR     0x00
+#define UBX_ID_INF_NOTICE    0x02
+#define UBX_ID_INF_WARNING   0x01
+#define UBX_ID_ACK_NAK       0x00
+#define UBX_ID_ACK_ACK       0x01
+#define UBX_ID_CFG_PRT       0x00 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_MSG       0x01 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_RATE      0x08 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_CFG       0x09 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_NAV5      0x24 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_RST       0x04
+#define UBX_ID_CFG_SBAS      0x16
+#define UBX_ID_CFG_TMODE3    0x71 // deprecated in protocol version >= 27 -> use CFG_VALSET
+#define UBX_ID_CFG_GNSS      0x3E
+#define UBX_ID_CFG_VALSET    0x8A
+#define UBX_ID_CFG_VALGET    0x8B
+#define UBX_ID_CFG_VALDEL    0x8C
+#define UBX_ID_MON_VER       0x04
+#define UBX_ID_MON_HW        0x09 // deprecated in protocol version >= 27 -> use MON_RF
+#define UBX_ID_MON_RF        0x38
 
 /* UBX ID for RTCM3 output messages */
 /* Minimal messages for RTK: 1005, 1077 + (1087 or 1127) */
 /* Reduced message size using MSM4: 1005, 1074 + (1084 or 1124)  */
-#define UBX_ID_RTCM3_1005     0x05    /**< Stationary RTK reference station ARP */
-#define UBX_ID_RTCM3_1074     0x4A    /**< GPS MSM4 */
-#define UBX_ID_RTCM3_1077     0x4D    /**< GPS MSM7 */
-#define UBX_ID_RTCM3_1084     0x54    /**< GLONASS MSM4 */
-#define UBX_ID_RTCM3_1087     0x57    /**< GLONASS MSM7 */
-#define UBX_ID_RTCM3_1094     0x5E    /**< Galileo MSM4 */
-#define UBX_ID_RTCM3_1097     0x61    /**< Galileo MSM7 */
-#define UBX_ID_RTCM3_1124     0x7C    /**< BeiDou MSM4 */
-#define UBX_ID_RTCM3_1127     0x7F    /**< BeiDou MSM7 */
-#define UBX_ID_RTCM3_1230     0xE6    /**< GLONASS code-phase biases */
-#define UBX_ID_RTCM3_4072     0xFE    /**< Reference station PVT (u-blox proprietary RTCM Message) - Used for moving baseline */
+#define UBX_ID_RTCM3_1005 0x05 /**< Stationary RTK reference station ARP */
+#define UBX_ID_RTCM3_1074 0x4A /**< GPS MSM4 */
+#define UBX_ID_RTCM3_1077 0x4D /**< GPS MSM7 */
+#define UBX_ID_RTCM3_1084 0x54 /**< GLONASS MSM4 */
+#define UBX_ID_RTCM3_1087 0x57 /**< GLONASS MSM7 */
+#define UBX_ID_RTCM3_1094 0x5E /**< Galileo MSM4 */
+#define UBX_ID_RTCM3_1097 0x61 /**< Galileo MSM7 */
+#define UBX_ID_RTCM3_1124 0x7C /**< BeiDou MSM4 */
+#define UBX_ID_RTCM3_1127 0x7F /**< BeiDou MSM7 */
+#define UBX_ID_RTCM3_1230 0xE6 /**< GLONASS code-phase biases */
+#define UBX_ID_RTCM3_4072 0xFE /**< Reference station PVT (u-blox proprietary RTCM Message) - Used for moving baseline */
 
 
 /* Message Classes & IDs */
@@ -177,242 +154,242 @@
 
 /* RX NAV_STATUS message content details */
 /*   Bitfield "flags" masks */
-#define UBX_RX_NAV_STATUS_SPOOFDETSTATE_MASK    0b00011000 /**< spoofDetState (Spoofing detection state) */
-#define UBX_RX_NAV_STATUS_SPOOFDETSTATE_SHIFT   3
+#define UBX_RX_NAV_STATUS_SPOOFDETSTATE_MASK  0b00011000 /**< spoofDetState (Spoofing detection state) */
+#define UBX_RX_NAV_STATUS_SPOOFDETSTATE_SHIFT 3
 
 /* RX NAV-PVT message content details */
 /*   Bitfield "valid" masks */
-#define UBX_RX_NAV_PVT_VALID_VALIDDATE          0x01    /**< validDate (Valid UTC Date) */
-#define UBX_RX_NAV_PVT_VALID_VALIDTIME          0x02    /**< validTime (Valid UTC Time) */
-#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED      0x04    /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
+#define UBX_RX_NAV_PVT_VALID_VALIDDATE     0x01 /**< validDate (Valid UTC Date) */
+#define UBX_RX_NAV_PVT_VALID_VALIDTIME     0x02 /**< validTime (Valid UTC Time) */
+#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
 
 /*   Bitfield "flags" masks */
-#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK          0x01    /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
-#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN           0x02    /**< diffSoln (1 if differential corrections were applied) */
-#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE           0x1C    /**< psmState (Power Save Mode state (see Power Management)) */
-#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID       0x20    /**< headVehValid (Heading of vehicle is valid) */
-#define UBX_RX_NAV_PVT_FLAGS_CARRSOLN           0xC0    /**< Carrier phase range solution (RTK mode) */
+#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK    0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
+#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN     0x02 /**< diffSoln (1 if differential corrections were applied) */
+#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE     0x1C /**< psmState (Power Save Mode state (see Power Management)) */
+#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
+#define UBX_RX_NAV_PVT_FLAGS_CARRSOLN     0xC0 /**< Carrier phase range solution (RTK mode) */
 
 /* RX NAV-TIMEUTC message content details */
 /*   Bitfield "valid" masks */
-#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW       0x01    /**< validTOW (1 = Valid Time of Week) */
-#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN       0x02    /**< validWKN (1 = Valid Week Number) */
-#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC       0x04    /**< validUTC (1 = Valid UTC Time) */
-#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD    0xF0    /**< utcStandard (0..15 = UTC standard identifier) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW    0x01 /**< validTOW (1 = Valid Time of Week) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN    0x02 /**< validWKN (1 = Valid Week Number) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC    0x04 /**< validUTC (1 = Valid UTC Time) */
+#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD 0xF0 /**< utcStandard (0..15 = UTC standard identifier) */
 
 /* TX CFG-PRT message contents
  * Note: not used with protocol version 27+ anymore
  */
-#define UBX_TX_CFG_PRT_PORTID                   0x01            /**< UART1 */
-#define UBX_TX_CFG_PRT_PORTID_USB               0x03            /**< USB */
-#define UBX_TX_CFG_PRT_PORTID_SPI               0x04            /**< SPI */
-#define UBX_TX_CFG_PRT_MODE                     0x000008D0      /**< 0b0000100011010000: 8N1 */
-#define UBX_TX_CFG_PRT_MODE_SPI                 0x00000100
-#define UBX_TX_CFG_PRT_BAUDRATE                 38400           /**< choose 38400 as GPS baudrate (pre M8* boards only) */
-#define UBX_TX_CFG_PRT_PROTO_UBX                (1<<0)
-#define UBX_TX_CFG_PRT_PROTO_RTCM               (1<<5)
+#define UBX_TX_CFG_PRT_PORTID     0x01       /**< UART1 */
+#define UBX_TX_CFG_PRT_PORTID_USB 0x03       /**< USB */
+#define UBX_TX_CFG_PRT_PORTID_SPI 0x04       /**< SPI */
+#define UBX_TX_CFG_PRT_MODE       0x000008D0 /**< 0b0000100011010000: 8N1 */
+#define UBX_TX_CFG_PRT_MODE_SPI   0x00000100
+#define UBX_TX_CFG_PRT_BAUDRATE   38400      /**< choose 38400 as GPS baudrate (pre M8* boards only) */
+#define UBX_TX_CFG_PRT_PROTO_UBX  (1 << 0)
+#define UBX_TX_CFG_PRT_PROTO_RTCM (1 << 5)
 
-#define UBX_BAUDRATE_M8_AND_NEWER               115200 /**< baudrate for M8+ boards */
+#define UBX_BAUDRATE_M8_AND_NEWER 115200 /**< baudrate for M8+ boards */
 
 /* TX CFG-RATE message contents
  * Note: not used with protocol version 27+ anymore
  */
-#define UBX_TX_CFG_RATE_MEASINTERVAL            200     /**< 200ms for 5Hz (F9* boards use 10Hz) */
-#define UBX_TX_CFG_RATE_NAVRATE                 1       /**< cannot be changed */
-#define UBX_TX_CFG_RATE_TIMEREF                 0       /**< 0: UTC, 1: GPS time */
+#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz (F9* boards use 10Hz) */
+#define UBX_TX_CFG_RATE_NAVRATE      1   /**< cannot be changed */
+#define UBX_TX_CFG_RATE_TIMEREF      0   /**< 0: UTC, 1: GPS time */
 
 /* TX CFG-NAV5 message contents
  * Note: not used with protocol version 27+ anymore
  */
-#define UBX_TX_CFG_NAV5_MASK                    0x0005  /**< Only update dynamic model and fix mode */
-#define UBX_TX_CFG_NAV5_FIXMODE                 2       /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
+#define UBX_TX_CFG_NAV5_MASK    0x0005 /**< Only update dynamic model and fix mode */
+#define UBX_TX_CFG_NAV5_FIXMODE 2      /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
 
 /* TX CFG-RST message contents
  */
-#define UBX_TX_CFG_RST_BBR_MODE_HOT_START       0
-#define UBX_TX_CFG_RST_BBR_MODE_WARM_START      1
-#define UBX_TX_CFG_RST_BBR_MODE_COLD_START      0xFFFF
-#define UBX_TX_CFG_RST_MODE_HARDWARE            0
-#define UBX_TX_CFG_RST_MODE_SOFTWARE            1
+#define UBX_TX_CFG_RST_BBR_MODE_HOT_START  0
+#define UBX_TX_CFG_RST_BBR_MODE_WARM_START 1
+#define UBX_TX_CFG_RST_BBR_MODE_COLD_START 0xFFFF
+#define UBX_TX_CFG_RST_MODE_HARDWARE       0
+#define UBX_TX_CFG_RST_MODE_SOFTWARE       1
 
 /* TX CFG-GNSS message contents
  */
-#define UBX_TX_CFG_GNSS_GNSSID_GPS              0       /**< gnssId of GPS */
-#define UBX_TX_CFG_GNSS_GNSSID_SBAS             1       /**< gnssId of SBAS */
-#define UBX_TX_CFG_GNSS_GNSSID_GALILEO          2       /**< gnssId of Galileo */
-#define UBX_TX_CFG_GNSS_GNSSID_BEIDOU           3       /**< gnssId of BeiDou */
-#define UBX_TX_CFG_GNSS_GNSSID_IMES             4       /**< gnssId of IMES */
-#define UBX_TX_CFG_GNSS_GNSSID_QZSS             5       /**< gnssId of QZSS */
-#define UBX_TX_CFG_GNSS_GNSSID_GLONASS          6       /**< gnssId of GLONASS */
-#define UBX_TX_CFG_GNSS_FLAGS_ENABLE            0x00000001  /**< Enable this GNSS system */
-#define UBX_TX_CFG_GNSS_FLAGS_GPS_L1CA          0x00010000  /**< GPS: Use L1C/A Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GPS_L2C           0x00100000  /**< GPS: Use L2C Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GPS_L5            0x00200000  /**< GPS: Use L5 Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_SBAS_L1CA         0x00010000  /**< SBAS: Use L1C/A Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E1        0x00010000  /**< Galileo: Use E1 Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E5A       0x00100000  /**< Galileo: Use E5a Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E5B       0x00200000  /**< Galileo: Use E5b Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B1I        0x00010000  /**< BeiDou: Use B1I Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B2I        0x00100000  /**< BeiDou: Use B2I Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B2A        0x00800000  /**< BeiDou: Use B2A Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_IMES_L1           0x00010000  /**< IMES: Use L1 Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L1CA         0x00010000  /**< QZSS: Use L1C/A Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L1S          0x00040000  /**< QZSS: Use L1S Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L2C          0x00100000  /**< QZSS: Use L2C Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L5           0x00200000  /**< QZSS: Use L5 Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GLONASS_L1        0x00010000  /**< GLONASS: Use L1 Signal */
-#define UBX_TX_CFG_GNSS_FLAGS_GLONASS_L2        0x00100000  /**< GLONASS: Use L2 Signal */
+#define UBX_TX_CFG_GNSS_GNSSID_GPS        0          /**< gnssId of GPS */
+#define UBX_TX_CFG_GNSS_GNSSID_SBAS       1          /**< gnssId of SBAS */
+#define UBX_TX_CFG_GNSS_GNSSID_GALILEO    2          /**< gnssId of Galileo */
+#define UBX_TX_CFG_GNSS_GNSSID_BEIDOU     3          /**< gnssId of BeiDou */
+#define UBX_TX_CFG_GNSS_GNSSID_IMES       4          /**< gnssId of IMES */
+#define UBX_TX_CFG_GNSS_GNSSID_QZSS       5          /**< gnssId of QZSS */
+#define UBX_TX_CFG_GNSS_GNSSID_GLONASS    6          /**< gnssId of GLONASS */
+#define UBX_TX_CFG_GNSS_FLAGS_ENABLE      0x00000001 /**< Enable this GNSS system */
+#define UBX_TX_CFG_GNSS_FLAGS_GPS_L1CA    0x00010000 /**< GPS: Use L1C/A Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GPS_L2C     0x00100000 /**< GPS: Use L2C Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GPS_L5      0x00200000 /**< GPS: Use L5 Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_SBAS_L1CA   0x00010000 /**< SBAS: Use L1C/A Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E1  0x00010000 /**< Galileo: Use E1 Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E5A 0x00100000 /**< Galileo: Use E5a Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GALILEO_E5B 0x00200000 /**< Galileo: Use E5b Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B1I  0x00010000 /**< BeiDou: Use B1I Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B2I  0x00100000 /**< BeiDou: Use B2I Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_BEIDOU_B2A  0x00800000 /**< BeiDou: Use B2A Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_IMES_L1     0x00010000 /**< IMES: Use L1 Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L1CA   0x00010000 /**< QZSS: Use L1C/A Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L1S    0x00040000 /**< QZSS: Use L1S Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L2C    0x00100000 /**< QZSS: Use L2C Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_QZSS_L5     0x00200000 /**< QZSS: Use L5 Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GLONASS_L1  0x00010000 /**< GLONASS: Use L1 Signal */
+#define UBX_TX_CFG_GNSS_FLAGS_GLONASS_L2  0x00100000 /**< GLONASS: Use L2 Signal */
 
 /* Key ID's for CFG-VAL{GET,SET,DEL} */
-#define UBX_CFG_KEY_CFG_I2C_ENABLED             0x10510003
-#define UBX_CFG_KEY_CFG_I2CINPROT_UBX           0x10710001
-#define UBX_CFG_KEY_CFG_I2CINPROT_NMEA          0x10710002
-#define UBX_CFG_KEY_CFG_I2CINPROT_RTCM3X        0x10710004
-#define UBX_CFG_KEY_CFG_I2COUTPROT_UBX          0x10720001
-#define UBX_CFG_KEY_CFG_I2COUTPROT_NMEA         0x10720002
-#define UBX_CFG_KEY_CFG_I2COUTPROT_RTCM3X       0x10720004
-
-#define UBX_CFG_KEY_CFG_UART1_BAUDRATE          0x40520001
-#define UBX_CFG_KEY_CFG_UART1_STOPBITS          0x20520002
-#define UBX_CFG_KEY_CFG_UART1_DATABITS          0x20520003
-#define UBX_CFG_KEY_CFG_UART1_PARITY            0x20520004
-#define UBX_CFG_KEY_CFG_UART1_ENABLED           0x20520005
-#define UBX_CFG_KEY_CFG_UART1_REMAP             0x20520006
-#define UBX_CFG_KEY_CFG_UART1INPROT_UBX         0x10730001
-#define UBX_CFG_KEY_CFG_UART1INPROT_NMEA        0x10730002
-#define UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X      0x10730004
-#define UBX_CFG_KEY_CFG_UART1OUTPROT_UBX        0x10740001
-#define UBX_CFG_KEY_CFG_UART1OUTPROT_NMEA       0x10740002
-#define UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X     0x10740004
-
-#define UBX_CFG_KEY_CFG_UART2_BAUDRATE          0x40530001
-#define UBX_CFG_KEY_CFG_UART2_STOPBITS          0x20530002
-#define UBX_CFG_KEY_CFG_UART2_DATABITS          0x20530003
-#define UBX_CFG_KEY_CFG_UART2_PARITY            0x20530004
-#define UBX_CFG_KEY_CFG_UART2_ENABLED           0x20530005
-#define UBX_CFG_KEY_CFG_UART2_REMAP             0x20530006
-#define UBX_CFG_KEY_CFG_UART2INPROT_UBX         0x10750001
-#define UBX_CFG_KEY_CFG_UART2INPROT_NMEA        0x10750002
-#define UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X      0x10750004
-#define UBX_CFG_KEY_CFG_UART2OUTPROT_UBX        0x10760001
-#define UBX_CFG_KEY_CFG_UART2OUTPROT_NMEA       0x10760002
-#define UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X     0x10760004
-
-#define UBX_CFG_KEY_CFG_USB_ENABLED             0x10650001
-#define UBX_CFG_KEY_CFG_USBINPROT_UBX           0x10770001
-#define UBX_CFG_KEY_CFG_USBINPROT_NMEA          0x10770002
-#define UBX_CFG_KEY_CFG_USBINPROT_RTCM3X        0x10770004
-#define UBX_CFG_KEY_CFG_USBOUTPROT_UBX          0x10780001
-#define UBX_CFG_KEY_CFG_USBOUTPROT_NMEA         0x10780002
-#define UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X       0x10780004
-
-#define UBX_CFG_KEY_CFG_SPIINPROT_UBX           0x10790001
-#define UBX_CFG_KEY_CFG_SPIINPROT_NMEA          0x10790002
-#define UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X        0x10790004
-#define UBX_CFG_KEY_CFG_SPIOUTPROT_UBX          0x107a0001
-#define UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA         0x107a0002
-#define UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X       0x107a0004
-
-#define UBX_CFG_KEY_NAVHPG_DGNSSMODE            0x20140011
-
-#define UBX_CFG_KEY_NAVSPG_FIXMODE              0x20110011
-#define UBX_CFG_KEY_NAVSPG_UTCSTANDARD          0x2011001c
-#define UBX_CFG_KEY_NAVSPG_DYNMODEL             0x20110021
-
-#define UBX_CFG_KEY_ODO_USE_ODO                 0x10220001
-#define UBX_CFG_KEY_ODO_USE_COG                 0x10220002
-#define UBX_CFG_KEY_ODO_OUTLPVEL                0x10220003
-#define UBX_CFG_KEY_ODO_OUTLPCOG                0x10220004
-
-#define UBX_CFG_KEY_ITFM_ENABLE                 0x1041000d
-
-#define UBX_CFG_KEY_RATE_MEAS                   0x30210001
-#define UBX_CFG_KEY_RATE_NAV                    0x30210002
-#define UBX_CFG_KEY_RATE_TIMEREF                0x20210003
-
-#define UBX_CFG_KEY_TMODE_MODE                  0x20030001
-#define UBX_CFG_KEY_TMODE_POS_TYPE              0x20030002
-#define UBX_CFG_KEY_TMODE_LAT                   0x40030009
-#define UBX_CFG_KEY_TMODE_LON                   0x4003000a
-#define UBX_CFG_KEY_TMODE_HEIGHT                0x4003000b
-#define UBX_CFG_KEY_TMODE_LAT_HP                0x2003000c
-#define UBX_CFG_KEY_TMODE_LON_HP                0x2003000d
-#define UBX_CFG_KEY_TMODE_HEIGHT_HP             0x2003000e
-#define UBX_CFG_KEY_TMODE_FIXED_POS_ACC         0x4003000f
-#define UBX_CFG_KEY_TMODE_SVIN_MIN_DUR          0x40030010
-#define UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT        0x40030011
-
-#define UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C        0x20910359
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C      0x20910088
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C       0x20910015
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_STATUS_I2C    0x2091001a
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C       0x20910038
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C       0x20910006
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_I2C 0x2091008d
-#define UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C     0x20910231
-#define UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C      0x209102a4
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C  0x209102bd
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C  0x209102cc
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C  0x209102d1
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C  0x20910318
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C  0x209102d6
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C  0x20910303
-
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1 0x2091008e
-#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2 0x2091008f
-
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1  0x209102ff
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART1  0x20910382
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART1    0x209102cd
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_UART1    0x209102d2
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_UART1    0x20910319
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_UART1    0x209102d7
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1    0x20910304
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1    0x2091035f
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1    0x20910364
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1    0x20910369
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1    0x2091036e
-
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART2  0x20910300
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART2  0x20910383
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART2    0x209102ce
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_UART2    0x209102d3
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_UART2    0x2091031a
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_UART2    0x209102d8
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART2    0x20910305
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART2    0x20910360
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART2    0x20910365
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART2    0x2091036a
-#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART2    0x2091036f
-
-#define UBX_CFG_KEY_SPI_ENABLED                 0x10640006
-#define UBX_CFG_KEY_SPI_MAXFF                   0x20640001
-
-#define UBX_CFG_KEY_SIGNAL_GPS_ENA              0x1031001f  /**< GPS enable */
-#define UBX_CFG_KEY_SIGNAL_GPS_L1CA_ENA         0x10310001  /**< GPS L1C/A */
-#define UBX_CFG_KEY_SIGNAL_GPS_L2C_ENA          0x10310003  /**< GPS L2C (only on u-blox F9 platform products) */
-#define UBX_CFG_KEY_SIGNAL_SBAS_ENA             0x10310020  /**< SBAS enable */
-#define UBX_CFG_KEY_SIGNAL_SBAS_L1CA_ENA        0x10310005  /**< SBAS L1C/A */
-#define UBX_CFG_KEY_SIGNAL_GAL_ENA              0x10310021  /**< Galileo enable */
-#define UBX_CFG_KEY_SIGNAL_GAL_E1_ENA           0x10310007  /**< Galileo E1 */
-#define UBX_CFG_KEY_SIGNAL_GAL_E5B_ENA          0x1031000a  /**< Galileo E5b (only on u-blox F9 platform products) */
-#define UBX_CFG_KEY_SIGNAL_BDS_ENA              0x10310022  /**< BeiDou Enable */
-#define UBX_CFG_KEY_SIGNAL_BDS_B1_ENA           0x1031000d  /**< BeiDou B1I */
-#define UBX_CFG_KEY_SIGNAL_BDS_B2_ENA           0x1031000e  /**< BeiDou B2I (only on u-blox F9 platform products) */
-#define UBX_CFG_KEY_SIGNAL_QZSS_ENA             0x10310024  /**< QZSS enable */
-#define UBX_CFG_KEY_SIGNAL_QZSS_L1CA_ENA        0x10310012  /**< QZSS L1C/A */
-#define UBX_CFG_KEY_SIGNAL_QZSS_L1S_ENA         0x10310014  /**< QZSS L1S */
-#define UBX_CFG_KEY_SIGNAL_QZSS_L2C_ENA         0x10310015  /**< QZSS L2C (only on u-blox F9 platform products) */
-#define UBX_CFG_KEY_SIGNAL_GLO_ENA              0x10310025  /**< GLONASS enable */
-#define UBX_CFG_KEY_SIGNAL_GLO_L1_ENA           0x10310018  /**< GLONASS L1 */
-#define UBX_CFG_KEY_SIGNAL_GLO_L2_ENA           0x1031001a  /**< GLONASS L2 (only on u-blox F9 platform products) */
-
-#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7        (sizeof(ubx_payload_rx_nav_pvt_t) - 8)
-#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8        (sizeof(ubx_payload_rx_nav_pvt_t))
-
-#define UBX_CFG_LAYER_RAM                       (1 << 0)
-#define UBX_CFG_LAYER_BBR                       (1 << 1)
-#define UBX_CFG_LAYER_FLASH                     (1 << 2)
+#define UBX_CFG_KEY_CFG_I2C_ENABLED                 0x10510003
+#define UBX_CFG_KEY_CFG_I2CINPROT_UBX               0x10710001
+#define UBX_CFG_KEY_CFG_I2CINPROT_NMEA              0x10710002
+#define UBX_CFG_KEY_CFG_I2CINPROT_RTCM3X            0x10710004
+#define UBX_CFG_KEY_CFG_I2COUTPROT_UBX              0x10720001
+#define UBX_CFG_KEY_CFG_I2COUTPROT_NMEA             0x10720002
+#define UBX_CFG_KEY_CFG_I2COUTPROT_RTCM3X           0x10720004
+
+#define UBX_CFG_KEY_CFG_UART1_BAUDRATE              0x40520001
+#define UBX_CFG_KEY_CFG_UART1_STOPBITS              0x20520002
+#define UBX_CFG_KEY_CFG_UART1_DATABITS              0x20520003
+#define UBX_CFG_KEY_CFG_UART1_PARITY                0x20520004
+#define UBX_CFG_KEY_CFG_UART1_ENABLED               0x20520005
+#define UBX_CFG_KEY_CFG_UART1_REMAP                 0x20520006
+#define UBX_CFG_KEY_CFG_UART1INPROT_UBX             0x10730001
+#define UBX_CFG_KEY_CFG_UART1INPROT_NMEA            0x10730002
+#define UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X          0x10730004
+#define UBX_CFG_KEY_CFG_UART1OUTPROT_UBX            0x10740001
+#define UBX_CFG_KEY_CFG_UART1OUTPROT_NMEA           0x10740002
+#define UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X         0x10740004
+
+#define UBX_CFG_KEY_CFG_UART2_BAUDRATE              0x40530001
+#define UBX_CFG_KEY_CFG_UART2_STOPBITS              0x20530002
+#define UBX_CFG_KEY_CFG_UART2_DATABITS              0x20530003
+#define UBX_CFG_KEY_CFG_UART2_PARITY                0x20530004
+#define UBX_CFG_KEY_CFG_UART2_ENABLED               0x20530005
+#define UBX_CFG_KEY_CFG_UART2_REMAP                 0x20530006
+#define UBX_CFG_KEY_CFG_UART2INPROT_UBX             0x10750001
+#define UBX_CFG_KEY_CFG_UART2INPROT_NMEA            0x10750002
+#define UBX_CFG_KEY_CFG_UART2INPROT_RTCM3X          0x10750004
+#define UBX_CFG_KEY_CFG_UART2OUTPROT_UBX            0x10760001
+#define UBX_CFG_KEY_CFG_UART2OUTPROT_NMEA           0x10760002
+#define UBX_CFG_KEY_CFG_UART2OUTPROT_RTCM3X         0x10760004
+
+#define UBX_CFG_KEY_CFG_USB_ENABLED                 0x10650001
+#define UBX_CFG_KEY_CFG_USBINPROT_UBX               0x10770001
+#define UBX_CFG_KEY_CFG_USBINPROT_NMEA              0x10770002
+#define UBX_CFG_KEY_CFG_USBINPROT_RTCM3X            0x10770004
+#define UBX_CFG_KEY_CFG_USBOUTPROT_UBX              0x10780001
+#define UBX_CFG_KEY_CFG_USBOUTPROT_NMEA             0x10780002
+#define UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X           0x10780004
+
+#define UBX_CFG_KEY_CFG_SPIINPROT_UBX               0x10790001
+#define UBX_CFG_KEY_CFG_SPIINPROT_NMEA              0x10790002
+#define UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X            0x10790004
+#define UBX_CFG_KEY_CFG_SPIOUTPROT_UBX              0x107a0001
+#define UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA             0x107a0002
+#define UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X           0x107a0004
+
+#define UBX_CFG_KEY_NAVHPG_DGNSSMODE                0x20140011
+
+#define UBX_CFG_KEY_NAVSPG_FIXMODE                  0x20110011
+#define UBX_CFG_KEY_NAVSPG_UTCSTANDARD              0x2011001c
+#define UBX_CFG_KEY_NAVSPG_DYNMODEL                 0x20110021
+
+#define UBX_CFG_KEY_ODO_USE_ODO                     0x10220001
+#define UBX_CFG_KEY_ODO_USE_COG                     0x10220002
+#define UBX_CFG_KEY_ODO_OUTLPVEL                    0x10220003
+#define UBX_CFG_KEY_ODO_OUTLPCOG                    0x10220004
+
+#define UBX_CFG_KEY_ITFM_ENABLE                     0x1041000d
+
+#define UBX_CFG_KEY_RATE_MEAS                       0x30210001
+#define UBX_CFG_KEY_RATE_NAV                        0x30210002
+#define UBX_CFG_KEY_RATE_TIMEREF                    0x20210003
+
+#define UBX_CFG_KEY_TMODE_MODE                      0x20030001
+#define UBX_CFG_KEY_TMODE_POS_TYPE                  0x20030002
+#define UBX_CFG_KEY_TMODE_LAT                       0x40030009
+#define UBX_CFG_KEY_TMODE_LON                       0x4003000a
+#define UBX_CFG_KEY_TMODE_HEIGHT                    0x4003000b
+#define UBX_CFG_KEY_TMODE_LAT_HP                    0x2003000c
+#define UBX_CFG_KEY_TMODE_LON_HP                    0x2003000d
+#define UBX_CFG_KEY_TMODE_HEIGHT_HP                 0x2003000e
+#define UBX_CFG_KEY_TMODE_FIXED_POS_ACC             0x4003000f
+#define UBX_CFG_KEY_TMODE_SVIN_MIN_DUR              0x40030010
+#define UBX_CFG_KEY_TMODE_SVIN_ACC_LIMIT            0x40030011
+
+#define UBX_CFG_KEY_MSGOUT_UBX_MON_RF_I2C           0x20910359
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SVIN_I2C         0x20910088
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_SAT_I2C          0x20910015
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_STATUS_I2C       0x2091001a
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_DOP_I2C          0x20910038
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_PVT_I2C          0x20910006
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_I2C    0x2091008d
+#define UBX_CFG_KEY_MSGOUT_UBX_RXM_SFRBX_I2C        0x20910231
+#define UBX_CFG_KEY_MSGOUT_UBX_RXM_RAWX_I2C         0x209102a4
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1005_I2C     0x209102bd
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_I2C     0x209102cc
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_I2C     0x209102d1
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_I2C     0x20910318
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_I2C     0x209102d6
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_I2C     0x20910303
+
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1  0x2091008e
+#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2  0x2091008f
+
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1 0x209102ff
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART1 0x20910382
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART1   0x209102cd
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_UART1   0x209102d2
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_UART1   0x20910319
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_UART1   0x209102d7
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART1   0x20910304
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART1   0x2091035f
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART1   0x20910364
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART1   0x20910369
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART1   0x2091036e
+
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART2 0x20910300
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART2 0x20910383
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART2   0x209102ce
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1087_UART2   0x209102d3
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1097_UART2   0x2091031a
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1127_UART2   0x209102d8
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1230_UART2   0x20910305
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1074_UART2   0x20910360
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1084_UART2   0x20910365
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1094_UART2   0x2091036a
+#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1124_UART2   0x2091036f
+
+#define UBX_CFG_KEY_SPI_ENABLED                     0x10640006
+#define UBX_CFG_KEY_SPI_MAXFF                       0x20640001
+
+#define UBX_CFG_KEY_SIGNAL_GPS_ENA                  0x1031001f /**< GPS enable */
+#define UBX_CFG_KEY_SIGNAL_GPS_L1CA_ENA             0x10310001 /**< GPS L1C/A */
+#define UBX_CFG_KEY_SIGNAL_GPS_L2C_ENA              0x10310003 /**< GPS L2C (only on u-blox F9 platform products) */
+#define UBX_CFG_KEY_SIGNAL_SBAS_ENA                 0x10310020 /**< SBAS enable */
+#define UBX_CFG_KEY_SIGNAL_SBAS_L1CA_ENA            0x10310005 /**< SBAS L1C/A */
+#define UBX_CFG_KEY_SIGNAL_GAL_ENA                  0x10310021 /**< Galileo enable */
+#define UBX_CFG_KEY_SIGNAL_GAL_E1_ENA               0x10310007 /**< Galileo E1 */
+#define UBX_CFG_KEY_SIGNAL_GAL_E5B_ENA              0x1031000a /**< Galileo E5b (only on u-blox F9 platform products) */
+#define UBX_CFG_KEY_SIGNAL_BDS_ENA                  0x10310022 /**< BeiDou Enable */
+#define UBX_CFG_KEY_SIGNAL_BDS_B1_ENA               0x1031000d /**< BeiDou B1I */
+#define UBX_CFG_KEY_SIGNAL_BDS_B2_ENA               0x1031000e /**< BeiDou B2I (only on u-blox F9 platform products) */
+#define UBX_CFG_KEY_SIGNAL_QZSS_ENA                 0x10310024 /**< QZSS enable */
+#define UBX_CFG_KEY_SIGNAL_QZSS_L1CA_ENA            0x10310012 /**< QZSS L1C/A */
+#define UBX_CFG_KEY_SIGNAL_QZSS_L1S_ENA             0x10310014 /**< QZSS L1S */
+#define UBX_CFG_KEY_SIGNAL_QZSS_L2C_ENA             0x10310015 /**< QZSS L2C (only on u-blox F9 platform products) */
+#define UBX_CFG_KEY_SIGNAL_GLO_ENA                  0x10310025 /**< GLONASS enable */
+#define UBX_CFG_KEY_SIGNAL_GLO_L1_ENA               0x10310018 /**< GLONASS L1 */
+#define UBX_CFG_KEY_SIGNAL_GLO_L2_ENA               0x1031001a /**< GLONASS L2 (only on u-blox F9 platform products) */
+
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7            (sizeof(ubx_payload_rx_nav_pvt_t) - 8)
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8            (sizeof(ubx_payload_rx_nav_pvt_t))
+
+#define UBX_CFG_LAYER_RAM                           (1 << 0)
+#define UBX_CFG_LAYER_BBR                           (1 << 1)
+#define UBX_CFG_LAYER_FLASH                         (1 << 2)
 
 // Forward Declaration
 class RTCMParsing;
@@ -423,603 +400,610 @@ class RTCMParsing;
 
 /* General: Header */
 typedef struct {
-	uint8_t  sync1;
-	uint8_t  sync2;
-	uint16_t msg;
-	uint16_t length;
+    uint8_t  sync1;
+    uint8_t  sync2;
+    uint16_t msg;
+    uint16_t length;
 } ubx_header_t;
 
 /* General: Checksum */
 typedef struct {
-	uint8_t ck_a;
-	uint8_t ck_b;
-} ubx_checksum_t ;
+    uint8_t ck_a;
+    uint8_t ck_b;
+} ubx_checksum_t;
 
 /* Rx NAV-POSLLH */
 typedef struct {
-	uint32_t iTOW;   /**< GPS Time of Week [ms] */
-	int32_t  lon;    /**< Longitude [1e-7 deg] */
-	int32_t  lat;    /**< Latitude [1e-7 deg] */
-	int32_t  height; /**< Height above ellipsoid [mm] */
-	int32_t  hMSL;   /**< Height above mean sea level [mm] */
-	uint32_t hAcc;   /**< Horizontal accuracy estimate [mm] */
-	uint32_t vAcc;   /**< Vertical accuracy estimate [mm] */
+    uint32_t iTOW;   /**< GPS Time of Week [ms] */
+    int32_t  lon;    /**< Longitude [1e-7 deg] */
+    int32_t  lat;    /**< Latitude [1e-7 deg] */
+    int32_t  height; /**< Height above ellipsoid [mm] */
+    int32_t  hMSL;   /**< Height above mean sea level [mm] */
+    uint32_t hAcc;   /**< Horizontal accuracy estimate [mm] */
+    uint32_t vAcc;   /**< Vertical accuracy estimate [mm] */
 } ubx_payload_rx_nav_posllh_t;
 
 /* Rx NAV-DOP */
 typedef struct {
-	uint32_t iTOW; /**< GPS Time of Week [ms] */
-	uint16_t gDOP; /**< Geometric DOP [0.01] */
-	uint16_t pDOP; /**< Position DOP [0.01] */
-	uint16_t tDOP; /**< Time DOP [0.01] */
-	uint16_t vDOP; /**< Vertical DOP [0.01] */
-	uint16_t hDOP; /**< Horizontal DOP [0.01] */
-	uint16_t nDOP; /**< Northing DOP [0.01] */
-	uint16_t eDOP; /**< Easting DOP [0.01] */
+    uint32_t iTOW; /**< GPS Time of Week [ms] */
+    uint16_t gDOP; /**< Geometric DOP [0.01] */
+    uint16_t pDOP; /**< Position DOP [0.01] */
+    uint16_t tDOP; /**< Time DOP [0.01] */
+    uint16_t vDOP; /**< Vertical DOP [0.01] */
+    uint16_t hDOP; /**< Horizontal DOP [0.01] */
+    uint16_t nDOP; /**< Northing DOP [0.01] */
+    uint16_t eDOP; /**< Easting DOP [0.01] */
 } ubx_payload_rx_nav_dop_t;
 
 /* Rx NAV-SOL */
 typedef struct {
-	uint32_t iTOW;     /**< GPS Time of Week [ms] */
-	int32_t  fTOW;     /**< Fractional part of iTOW (range: +/-500000) [ns] */
-	int16_t  week;     /**< GPS week */
-	uint8_t  gpsFix;   /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
-	uint8_t  flags;
-	int32_t  ecefX;
-	int32_t  ecefY;
-	int32_t  ecefZ;
-	uint32_t pAcc;
-	int32_t  ecefVX;
-	int32_t  ecefVY;
-	int32_t  ecefVZ;
-	uint32_t sAcc;
-	uint16_t pDOP;      /**< Position DOP [0.01] */
-	uint8_t  reserved1;
-	uint8_t  numSV;     /**< Number of SVs used in Nav Solution */
-	uint32_t reserved2;
+    uint32_t iTOW;   /**< GPS Time of Week [ms] */
+    int32_t  fTOW;   /**< Fractional part of iTOW (range: +/-500000) [ns] */
+    int16_t  week;   /**< GPS week */
+    uint8_t  gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+    uint8_t  flags;
+    int32_t  ecefX;
+    int32_t  ecefY;
+    int32_t  ecefZ;
+    uint32_t pAcc;
+    int32_t  ecefVX;
+    int32_t  ecefVY;
+    int32_t  ecefVZ;
+    uint32_t sAcc;
+    uint16_t pDOP;  /**< Position DOP [0.01] */
+    uint8_t  reserved1;
+    uint8_t  numSV; /**< Number of SVs used in Nav Solution */
+    uint32_t reserved2;
 } ubx_payload_rx_nav_sol_t;
 
 /* Rx NAV-PVT (ubx8) */
 typedef struct {
-	uint32_t iTOW;          /**< GPS Time of Week [ms] */
-	uint16_t year;          /**< Year (UTC)*/
-	uint8_t  month;         /**< Month, range 1..12 (UTC) */
-	uint8_t  day;           /**< Day of month, range 1..31 (UTC) */
-	uint8_t  hour;          /**< Hour of day, range 0..23 (UTC) */
-	uint8_t  min;           /**< Minute of hour, range 0..59 (UTC) */
-	uint8_t  sec;           /**< Seconds of minute, range 0..60 (UTC) */
-	uint8_t  valid;         /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
-	uint32_t tAcc;          /**< Time accuracy estimate (UTC) [ns] */
-	int32_t  nano;          /**< Fraction of second (UTC) [-1e9...1e9 ns] */
-	uint8_t  fixType;       /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
-	uint8_t  flags;         /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
-	uint8_t  reserved1;
-	uint8_t  numSV;         /**< Number of SVs used in Nav Solution */
-	int32_t  lon;           /**< Longitude [1e-7 deg] */
-	int32_t  lat;           /**< Latitude [1e-7 deg] */
-	int32_t  height;        /**< Height above ellipsoid [mm] */
-	int32_t  hMSL;          /**< Height above mean sea level [mm] */
-	uint32_t hAcc;          /**< Horizontal accuracy estimate [mm] */
-	uint32_t vAcc;          /**< Vertical accuracy estimate [mm] */
-	int32_t  velN;          /**< NED north velocity [mm/s]*/
-	int32_t  velE;          /**< NED east velocity [mm/s]*/
-	int32_t  velD;          /**< NED down velocity [mm/s]*/
-	int32_t  gSpeed;        /**< Ground Speed (2-D) [mm/s] */
-	int32_t  headMot;       /**< Heading of motion (2-D) [1e-5 deg] */
-	uint32_t sAcc;          /**< Speed accuracy estimate [mm/s] */
-	uint32_t headAcc;       /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
-	uint16_t pDOP;          /**< Position DOP [0.01] */
-	uint16_t reserved2;
-	uint32_t reserved3;
-	int32_t  headVeh;       /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
-	uint32_t reserved4;     /**< (ubx8+ only) */
+    uint32_t iTOW;    /**< GPS Time of Week [ms] */
+    uint16_t year;    /**< Year (UTC)*/
+    uint8_t  month;   /**< Month, range 1..12 (UTC) */
+    uint8_t  day;     /**< Day of month, range 1..31 (UTC) */
+    uint8_t  hour;    /**< Hour of day, range 0..23 (UTC) */
+    uint8_t  min;     /**< Minute of hour, range 0..59 (UTC) */
+    uint8_t  sec;     /**< Seconds of minute, range 0..60 (UTC) */
+    uint8_t  valid;   /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
+    uint32_t tAcc;    /**< Time accuracy estimate (UTC) [ns] */
+    int32_t  nano;    /**< Fraction of second (UTC) [-1e9...1e9 ns] */
+    uint8_t  fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
+    uint8_t  flags;   /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
+    uint8_t  reserved1;
+    uint8_t  numSV;   /**< Number of SVs used in Nav Solution */
+    int32_t  lon;     /**< Longitude [1e-7 deg] */
+    int32_t  lat;     /**< Latitude [1e-7 deg] */
+    int32_t  height;  /**< Height above ellipsoid [mm] */
+    int32_t  hMSL;    /**< Height above mean sea level [mm] */
+    uint32_t hAcc;    /**< Horizontal accuracy estimate [mm] */
+    uint32_t vAcc;    /**< Vertical accuracy estimate [mm] */
+    int32_t  velN;    /**< NED north velocity [mm/s]*/
+    int32_t  velE;    /**< NED east velocity [mm/s]*/
+    int32_t  velD;    /**< NED down velocity [mm/s]*/
+    int32_t  gSpeed;  /**< Ground Speed (2-D) [mm/s] */
+    int32_t  headMot; /**< Heading of motion (2-D) [1e-5 deg] */
+    uint32_t sAcc;    /**< Speed accuracy estimate [mm/s] */
+    uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
+    uint16_t pDOP;    /**< Position DOP [0.01] */
+    uint16_t reserved2;
+    uint32_t reserved3;
+    int32_t  headVeh;   /**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
+    uint32_t reserved4; /**< (ubx8+ only) */
 } ubx_payload_rx_nav_pvt_t;
 
 /* Rx NAV-TIMEUTC */
 typedef struct {
-	uint32_t iTOW;           /**< GPS Time of Week [ms] */
-	uint32_t tAcc;           /**< Time accuracy estimate (UTC) [ns] */
-	int32_t  nano;           /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
-	uint16_t year;           /**< Year, range 1999..2099 (UTC) */
-	uint8_t  month;          /**< Month, range 1..12 (UTC) */
-	uint8_t  day;            /**< Day of month, range 1..31 (UTC) */
-	uint8_t  hour;           /**< Hour of day, range 0..23 (UTC) */
-	uint8_t  min;            /**< Minute of hour, range 0..59 (UTC) */
-	uint8_t  sec;            /**< Seconds of minute, range 0..60 (UTC) */
-	uint8_t  valid;          /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
+    uint32_t iTOW;  /**< GPS Time of Week [ms] */
+    uint32_t tAcc;  /**< Time accuracy estimate (UTC) [ns] */
+    int32_t  nano;  /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
+    uint16_t year;  /**< Year, range 1999..2099 (UTC) */
+    uint8_t  month; /**< Month, range 1..12 (UTC) */
+    uint8_t  day;   /**< Day of month, range 1..31 (UTC) */
+    uint8_t  hour;  /**< Hour of day, range 0..23 (UTC) */
+    uint8_t  min;   /**< Minute of hour, range 0..59 (UTC) */
+    uint8_t  sec;   /**< Seconds of minute, range 0..60 (UTC) */
+    uint8_t  valid; /**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
 } ubx_payload_rx_nav_timeutc_t;
 
 /* Rx NAV-SVINFO Part 1 */
 typedef struct {
-	uint32_t iTOW;           /**< GPS Time of Week [ms] */
-	uint8_t  numCh;          /**< Number of channels */
-	uint8_t  globalFlags;
-	uint16_t reserved2;
+    uint32_t iTOW;  /**< GPS Time of Week [ms] */
+    uint8_t  numCh; /**< Number of channels */
+    uint8_t  globalFlags;
+    uint16_t reserved2;
 } ubx_payload_rx_nav_svinfo_part1_t;
 
 /* Rx NAV-SVINFO Part 2 (repeated) */
 typedef struct {
-	uint8_t chn;            /**< Channel number, 255 for SVs not assigned to a channel */
-	uint8_t svid;           /**< Satellite ID */
-	uint8_t flags;          /**< svUsed, diffCorr, orbitAvail, orbitEph, unhealthy, orbitAlm, orbitAop, smoothed */
-	uint8_t quality;        /**< 0: no signal, 1: search, 2: aquited, 3: unusable, 5-7: locked */
-	uint8_t cno;            /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
-	int8_t  elev;           /**< Elevation [deg] */
-	int16_t azim;           /**< Azimuth [deg] */
-	int32_t prRes;          /**< Pseudo range residual [cm] */
+    uint8_t chn;     /**< Channel number, 255 for SVs not assigned to a channel */
+    uint8_t svid;    /**< Satellite ID */
+    uint8_t flags;   /**< svUsed, diffCorr, orbitAvail, orbitEph, unhealthy, orbitAlm, orbitAop, smoothed */
+    uint8_t quality; /**< 0: no signal, 1: search, 2: aquited, 3: unusable, 5-7: locked */
+    uint8_t cno;     /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
+    int8_t  elev;    /**< Elevation [deg] */
+    int16_t azim;    /**< Azimuth [deg] */
+    int32_t prRes;   /**< Pseudo range residual [cm] */
 } ubx_payload_rx_nav_svinfo_part2_t;
 
 /* Rx NAV-SAT Part 1 */
 typedef struct {
-	uint32_t iTOW;           /**< GPS Time of Week [ms] */
-	uint8_t  version;        /**< Message version (1) */
-	uint8_t  numSvs;         /**< Number of Satellites */
-	uint16_t reserved;
+    uint32_t iTOW;    /**< GPS Time of Week [ms] */
+    uint8_t  version; /**< Message version (1) */
+    uint8_t  numSvs;  /**< Number of Satellites */
+    uint16_t reserved;
 } ubx_payload_rx_nav_sat_part1_t;
 
 /* Rx NAV-SAT Part 2 (repeated) */
 typedef struct {
-	uint8_t  gnssId;         /**< GNSS identifier */
-	uint8_t  svId;           /**< Satellite ID */
-	uint8_t  cno;            /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
-	int8_t   elev;           /**< Elevation [deg] range: +/-90 */
-	int16_t  azim;           /**< Azimuth [deg] range: 0-360 */
-	int16_t  prRes;          /**< Pseudo range residual [0.1 m] */
-	uint32_t flags;
+    uint8_t  gnssId; /**< GNSS identifier */
+    uint8_t  svId;   /**< Satellite ID */
+    uint8_t  cno;    /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
+    int8_t   elev;   /**< Elevation [deg] range: +/-90 */
+    int16_t  azim;   /**< Azimuth [deg] range: 0-360 */
+    int16_t  prRes;  /**< Pseudo range residual [0.1 m] */
+    uint32_t flags;
 } ubx_payload_rx_nav_sat_part2_t;
 
 /* Rx NAV-STATUS */
 typedef struct {
-	uint32_t iTOW;           /**< GPS Time of Week [ms] */
-	uint8_t  gpsFix;         /**< GPSfix Type, range 0..5 */
-	uint8_t  flags;          /**< Fix Status Flags */
-	uint8_t  fixStat;        /**< Fix Status Information */
-	uint8_t  flags2;         /**< Additional Flags */
-	uint32_t ttff;           /**< Time to first fix [ms] */
-	uint32_t msss;           /**< Milliseconds since startup/reset */
+    uint32_t iTOW;    /**< GPS Time of Week [ms] */
+    uint8_t  gpsFix;  /**< GPSfix Type, range 0..5 */
+    uint8_t  flags;   /**< Fix Status Flags */
+    uint8_t  fixStat; /**< Fix Status Information */
+    uint8_t  flags2;  /**< Additional Flags */
+    uint32_t ttff;    /**< Time to first fix [ms] */
+    uint32_t msss;    /**< Milliseconds since startup/reset */
 } ubx_payload_rx_nav_status_t;
 
 /* Rx NAV-SVIN (survey-in info) */
 typedef struct {
-	uint8_t  version;
-	uint8_t  reserved1[3];
-	uint32_t iTOW;
-	uint32_t dur;
-	int32_t  meanX;
-	int32_t  meanY;
-	int32_t  meanZ;
-	int8_t   meanXHP;
-	int8_t   meanYHP;
-	int8_t   meanZHP;
-	int8_t   reserved2;
-	uint32_t meanAcc;
-	uint32_t obs;
-	uint8_t  valid;
-	uint8_t  active;
-	uint8_t  reserved3[2];
+    uint8_t  version;
+    uint8_t  reserved1[3];
+    uint32_t iTOW;
+    uint32_t dur;
+    int32_t  meanX;
+    int32_t  meanY;
+    int32_t  meanZ;
+    int8_t   meanXHP;
+    int8_t   meanYHP;
+    int8_t   meanZHP;
+    int8_t   reserved2;
+    uint32_t meanAcc;
+    uint32_t obs;
+    uint8_t  valid;
+    uint8_t  active;
+    uint8_t  reserved3[2];
 } ubx_payload_rx_nav_svin_t;
 
 /* Rx NAV-VELNED */
 typedef struct {
-	uint32_t iTOW;           /**< GPS Time of Week [ms] */
-	int32_t  velN;           /**< North velocity component [cm/s]*/
-	int32_t  velE;           /**< East velocity component [cm/s]*/
-	int32_t  velD;           /**< Down velocity component [cm/s]*/
-	uint32_t speed;          /**< Speed (3-D) [cm/s] */
-	uint32_t gSpeed;         /**< Ground speed (2-D) [cm/s] */
-	int32_t  heading;        /**< Heading of motion 2-D [1e-5 deg] */
-	uint32_t sAcc;           /**< Speed accuracy estimate [cm/s] */
-	uint32_t cAcc;           /**< Course / Heading accuracy estimate [1e-5 deg] */
+    uint32_t iTOW;    /**< GPS Time of Week [ms] */
+    int32_t  velN;    /**< North velocity component [cm/s]*/
+    int32_t  velE;    /**< East velocity component [cm/s]*/
+    int32_t  velD;    /**< Down velocity component [cm/s]*/
+    uint32_t speed;   /**< Speed (3-D) [cm/s] */
+    uint32_t gSpeed;  /**< Ground speed (2-D) [cm/s] */
+    int32_t  heading; /**< Heading of motion 2-D [1e-5 deg] */
+    uint32_t sAcc;    /**< Speed accuracy estimate [cm/s] */
+    uint32_t cAcc;    /**< Course / Heading accuracy estimate [1e-5 deg] */
 } ubx_payload_rx_nav_velned_t;
 
 /* Rx MON-HW (ubx6) */
 typedef struct {
-	uint32_t pinSel;
-	uint32_t pinBank;
-	uint32_t pinDir;
-	uint32_t pinVal;
-	uint16_t noisePerMS;
-	uint16_t agcCnt;
-	uint8_t  aStatus;
-	uint8_t  aPower;
-	uint8_t  flags;
-	uint8_t  reserved1;
-	uint32_t usedMask;
-	uint8_t  VP[25];
-	uint8_t  jamInd;
-	uint16_t reserved3;
-	uint32_t pinIrq;
-	uint32_t pullH;
-	uint32_t pullL;
+    uint32_t pinSel;
+    uint32_t pinBank;
+    uint32_t pinDir;
+    uint32_t pinVal;
+    uint16_t noisePerMS;
+    uint16_t agcCnt;
+    uint8_t  aStatus;
+    uint8_t  aPower;
+    uint8_t  flags;
+    uint8_t  reserved1;
+    uint32_t usedMask;
+    uint8_t  VP[25];
+    uint8_t  jamInd;
+    uint16_t reserved3;
+    uint32_t pinIrq;
+    uint32_t pullH;
+    uint32_t pullL;
 } ubx_payload_rx_mon_hw_ubx6_t;
 
 /* Rx MON-HW (ubx7+) */
 typedef struct {
-	uint32_t pinSel;
-	uint32_t pinBank;
-	uint32_t pinDir;
-	uint32_t pinVal;
-	uint16_t noisePerMS;
-	uint16_t agcCnt;
-	uint8_t  aStatus;
-	uint8_t  aPower;
-	uint8_t  flags;
-	uint8_t  reserved1;
-	uint32_t usedMask;
-	uint8_t  VP[17];
-	uint8_t  jamInd;
-	uint16_t reserved3;
-	uint32_t pinIrq;
-	uint32_t pullH;
-	uint32_t pullL;
+    uint32_t pinSel;
+    uint32_t pinBank;
+    uint32_t pinDir;
+    uint32_t pinVal;
+    uint16_t noisePerMS;
+    uint16_t agcCnt;
+    uint8_t  aStatus;
+    uint8_t  aPower;
+    uint8_t  flags;
+    uint8_t  reserved1;
+    uint32_t usedMask;
+    uint8_t  VP[17];
+    uint8_t  jamInd;
+    uint16_t reserved3;
+    uint32_t pinIrq;
+    uint32_t pullH;
+    uint32_t pullL;
 } ubx_payload_rx_mon_hw_ubx7_t;
 
 /* Rx MON-RF (replaces MON-HW, protocol 27+) */
 typedef struct {
-	uint8_t version;
-	uint8_t nBlocks;         /**< number of RF blocks included */
-	uint8_t reserved1[2];
-
-	struct ubx_payload_rx_mon_rf_block_t {
-		uint8_t  blockId;       /**< RF block id */
-		uint8_t  flags;         /**< jammingState */
-		uint8_t  antStatus;     /**< Status of the antenna superior state machine */
-		uint8_t  antPower;      /**< Current power status of antenna */
-		uint32_t postStatus;    /**< POST status word */
-		uint8_t  reserved2[4];
-		uint16_t noisePerMS;    /**< Noise level as measured by the GPS core */
-		uint16_t agcCnt;        /**< AGC Monitor (counts SIGI xor SIGLO, range 0 to 8191 */
-		uint8_t  jamInd;        /**< CW jamming indicator, scaled (0=no CW jamming, 255=strong CW jamming) */
-		int8_t   ofsI;          /**< Imbalance of I-part of complex signal */
-		uint8_t  magI;          /**< Magnitude of I-part of complex signal (0=no signal, 255=max magnitude) */
-		int8_t   ofsQ;          /**< Imbalance of Q-part of complex signal */
-		uint8_t  magQ;          /**< Magnitude of Q-part of complex signal (0=no signal, 255=max magnitude) */
-		uint8_t  reserved3[3];
-	};
-
-	ubx_payload_rx_mon_rf_block_t block[1]; ///< only read out the first block
+    uint8_t version;
+    uint8_t nBlocks; /**< number of RF blocks included */
+    uint8_t reserved1[2];
+
+    struct ubx_payload_rx_mon_rf_block_t {
+        uint8_t  blockId;    /**< RF block id */
+        uint8_t  flags;      /**< jammingState */
+        uint8_t  antStatus;  /**< Status of the antenna superior state machine */
+        uint8_t  antPower;   /**< Current power status of antenna */
+        uint32_t postStatus; /**< POST status word */
+        uint8_t  reserved2[4];
+        uint16_t noisePerMS; /**< Noise level as measured by the GPS core */
+        uint16_t agcCnt;     /**< AGC Monitor (counts SIGI xor SIGLO, range 0 to 8191 */
+        uint8_t  jamInd;     /**< CW jamming indicator, scaled (0=no CW jamming, 255=strong CW jamming) */
+        int8_t   ofsI;       /**< Imbalance of I-part of complex signal */
+        uint8_t  magI;       /**< Magnitude of I-part of complex signal (0=no signal, 255=max magnitude) */
+        int8_t   ofsQ;       /**< Imbalance of Q-part of complex signal */
+        uint8_t  magQ;       /**< Magnitude of Q-part of complex signal (0=no signal, 255=max magnitude) */
+        uint8_t  reserved3[3];
+    };
+
+    ubx_payload_rx_mon_rf_block_t block[1]; ///< only read out the first block
 } ubx_payload_rx_mon_rf_t;
 
 /* Rx MON-VER Part 1 */
 typedef struct {
-	uint8_t swVersion[30];
-	uint8_t hwVersion[10];
+    uint8_t swVersion[30];
+    uint8_t hwVersion[10];
 } ubx_payload_rx_mon_ver_part1_t;
 
 /* Rx MON-VER Part 2 (repeated) */
 typedef struct {
-	uint8_t extension[30];
+    uint8_t extension[30];
 } ubx_payload_rx_mon_ver_part2_t;
 
 /* Rx ACK-ACK */
 typedef union {
-	uint16_t msg;
-	struct {
-		uint8_t clsID;
-		uint8_t msgID;
-	};
+    uint16_t msg;
+
+    struct {
+        uint8_t clsID;
+        uint8_t msgID;
+    };
 } ubx_payload_rx_ack_ack_t;
 
 /* Rx ACK-NAK */
 typedef union {
-	uint16_t msg;
-	struct {
-		uint8_t clsID;
-		uint8_t msgID;
-	};
+    uint16_t msg;
+
+    struct {
+        uint8_t clsID;
+        uint8_t msgID;
+    };
 } ubx_payload_rx_ack_nak_t;
 
 /* Tx CFG-PRT */
 typedef struct {
-	uint8_t  portID;
-	uint8_t  reserved0;
-	uint16_t txReady;
-	uint32_t mode;
-	uint32_t baudRate;
-	uint16_t inProtoMask;
-	uint16_t outProtoMask;
-	uint16_t flags;
-	uint16_t reserved5;
+    uint8_t  portID;
+    uint8_t  reserved0;
+    uint16_t txReady;
+    uint32_t mode;
+    uint32_t baudRate;
+    uint16_t inProtoMask;
+    uint16_t outProtoMask;
+    uint16_t flags;
+    uint16_t reserved5;
 } ubx_payload_tx_cfg_prt_t;
 
 /* Tx CFG-RATE */
 typedef struct {
-	uint16_t measRate;      /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
-	uint16_t navRate;       /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
-	uint16_t timeRef;       /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
+    uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
+    uint16_t navRate;  /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
+    uint16_t timeRef;  /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
 } ubx_payload_tx_cfg_rate_t;
 
 /* Tx CFG-CFG */
 typedef struct {
-	uint32_t clearMask;     /**< Clear settings */
-	uint32_t saveMask;      /**< Save settings */
-	uint32_t loadMask;      /**< Load settings */
-	uint8_t  deviceMask;    /**< Storage devices to apply this top */
+    uint32_t clearMask;  /**< Clear settings */
+    uint32_t saveMask;   /**< Save settings */
+    uint32_t loadMask;   /**< Load settings */
+    uint8_t  deviceMask; /**< Storage devices to apply this top */
 } ubx_payload_tx_cfg_cfg_t;
 
 /* Tx CFG-VALSET (protocol version 27+) */
 typedef struct {
-	uint8_t version;        /**< Message version, set to 0 */
-	uint8_t layers;         /**< The layers where the configuration should be applied (@see UBX_CFG_LAYER_*) */
-	uint8_t reserved1[2];
-	uint8_t cfgData;        /**< configuration data (key and value pairs, max 64) */
+    uint8_t version; /**< Message version, set to 0 */
+    uint8_t layers;  /**< The layers where the configuration should be applied (@see UBX_CFG_LAYER_*) */
+    uint8_t reserved1[2];
+    uint8_t cfgData; /**< configuration data (key and value pairs, max 64) */
 } ubx_payload_tx_cfg_valset_t;
 
 /* Tx CFG-NAV5 */
 typedef struct {
-	uint16_t mask;
-	uint8_t  dynModel;       /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
-	uint8_t  fixMode;        /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
-	int32_t  fixedAlt;
-	uint32_t fixedAltVar;
-	int8_t   minElev;
-	uint8_t  drLimit;
-	uint16_t pDop;
-	uint16_t tDop;
-	uint16_t pAcc;
-	uint16_t tAcc;
-	uint8_t  staticHoldThresh;
-	uint8_t  dgpsTimeOut;
-	uint8_t  cnoThreshNumSVs;        /**< (ubx7+ only, else 0) */
-	uint8_t  cnoThresh;              /**< (ubx7+ only, else 0) */
-	uint16_t reserved;
-	uint16_t staticHoldMaxDist;      /**< (ubx8+ only, else 0) */
-	uint8_t  utcStandard;            /**< (ubx8+ only, else 0) */
-	uint8_t  reserved3;
-	uint32_t reserved4;
+    uint16_t mask;
+    uint8_t  dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+    uint8_t  fixMode;  /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
+    int32_t  fixedAlt;
+    uint32_t fixedAltVar;
+    int8_t   minElev;
+    uint8_t  drLimit;
+    uint16_t pDop;
+    uint16_t tDop;
+    uint16_t pAcc;
+    uint16_t tAcc;
+    uint8_t  staticHoldThresh;
+    uint8_t  dgpsTimeOut;
+    uint8_t  cnoThreshNumSVs;   /**< (ubx7+ only, else 0) */
+    uint8_t  cnoThresh;         /**< (ubx7+ only, else 0) */
+    uint16_t reserved;
+    uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */
+    uint8_t  utcStandard;       /**< (ubx8+ only, else 0) */
+    uint8_t  reserved3;
+    uint32_t reserved4;
 } ubx_payload_tx_cfg_nav5_t;
 
 /* tx cfg-rst */
 typedef struct {
-	uint16_t navBbrMask;
-	uint8_t  resetMode;
-	uint8_t  reserved1;
+    uint16_t navBbrMask;
+    uint8_t  resetMode;
+    uint8_t  reserved1;
 } ubx_payload_tx_cfg_rst_t;
 
 /* tx cfg-sbas */
 typedef struct {
-	uint8_t  mode;
-	uint8_t  usage;
-	uint8_t  maxSBAS;
-	uint8_t  scanmode2;
-	uint32_t scanmode1;
+    uint8_t  mode;
+    uint8_t  usage;
+    uint8_t  maxSBAS;
+    uint8_t  scanmode2;
+    uint32_t scanmode1;
 } ubx_payload_tx_cfg_sbas_t;
 
 /* Tx CFG-MSG */
 typedef struct {
-	union {
-		uint16_t msg;
-		struct {
-			uint8_t msgClass;
-			uint8_t msgID;
-		};
-	};
-	uint8_t rate;
+    union {
+        uint16_t msg;
+
+        struct {
+            uint8_t msgClass;
+            uint8_t msgID;
+        };
+    };
+
+    uint8_t rate;
 } ubx_payload_tx_cfg_msg_t;
 
 /* CFG-TMODE3 ublox 8 (protocol version >= 20) */
 typedef struct {
-	uint8_t  version;
-	uint8_t  reserved1;
-	uint16_t flags;
-	int32_t  ecefXOrLat;
-	int32_t  ecefYOrLon;
-	int32_t  ecefZOrAlt;
-	int8_t   ecefXOrLatHP;
-	int8_t   ecefYOrLonHP;
-	int8_t   ecefZOrAltHP;
-	uint8_t  reserved2;
-	uint32_t fixedPosAcc;
-	uint32_t svinMinDur;
-	uint32_t svinAccLimit;
-	uint8_t  reserved3[8];
+    uint8_t  version;
+    uint8_t  reserved1;
+    uint16_t flags;
+    int32_t  ecefXOrLat;
+    int32_t  ecefYOrLon;
+    int32_t  ecefZOrAlt;
+    int8_t   ecefXOrLatHP;
+    int8_t   ecefYOrLonHP;
+    int8_t   ecefZOrAltHP;
+    uint8_t  reserved2;
+    uint32_t fixedPosAcc;
+    uint32_t svinMinDur;
+    uint32_t svinAccLimit;
+    uint8_t  reserved3[8];
 } ubx_payload_tx_cfg_tmode3_t;
 
 typedef struct {
-	uint8_t msgVer;           /**< Message version (expected 0x00) */
-	uint8_t numTrkChHw;       /**< Number of tracking channels available (read only) */
-	uint8_t numTrkChUse;      /**< Number of tracking channels to use (0xFF for numTrkChHw) */
-	uint8_t numConfigBlocks;  /**< Count of repeated blocks */
-
-	struct ubx_payload_tx_cgf_gnss_block_t {
-		uint8_t gnssId;     /**< GNSS ID */
-		uint8_t resTrkCh;   /**< Number of reseved (minimum) tracking channels */
-		uint8_t maxTrkCh;   /**< Maximum number or tracking channels */
-		uint8_t reserved1;
-		uint32_t flags;     /**< Bitfield flags (see UBX_TX_CFG_GNSS_FLAGS_*) */
-	};
-
-	ubx_payload_tx_cgf_gnss_block_t block[7];  /**< GPS, SBAS, Galileo, BeiDou, IMES 0-8, QZSS, GLONASS */
+    uint8_t msgVer;          /**< Message version (expected 0x00) */
+    uint8_t numTrkChHw;      /**< Number of tracking channels available (read only) */
+    uint8_t numTrkChUse;     /**< Number of tracking channels to use (0xFF for numTrkChHw) */
+    uint8_t numConfigBlocks; /**< Count of repeated blocks */
+
+    struct ubx_payload_tx_cgf_gnss_block_t {
+        uint8_t  gnssId;   /**< GNSS ID */
+        uint8_t  resTrkCh; /**< Number of reseved (minimum) tracking channels */
+        uint8_t  maxTrkCh; /**< Maximum number or tracking channels */
+        uint8_t  reserved1;
+        uint32_t flags;    /**< Bitfield flags (see UBX_TX_CFG_GNSS_FLAGS_*) */
+    };
+
+    ubx_payload_tx_cgf_gnss_block_t block[7]; /**< GPS, SBAS, Galileo, BeiDou, IMES 0-8, QZSS, GLONASS */
 } ubx_payload_tx_cfg_gnss_t;
 
 /* NAV RELPOSNED (protocol version 27+) */
 typedef struct {
-	uint8_t     version;         /**< message version (expected 0x01) */
-	uint8_t     reserved0;
-	uint16_t    refStationId;    /**< Reference station ID. Must be in the range 0..4095 */
-	uint32_t    iTOW;            /**< [ms] GPS time of week of the navigation epoch */
-	int32_t     relPosN;         /**< [cm] North component of relative position vector */
-	int32_t     relPosE;         /**< [cm] East component of relative position vector */
-	int32_t     relPosD;         /**< [cm] Down component of relative position vector */
-	int32_t     relPosLength;    /**< [cm] Length of the relative position vector */
-	int32_t     relPosHeading;   /**< [1e-5 deg] Heading of the relative position vector */
-	uint32_t    reserved1;
-	int8_t      relPosHPN;       /**< [0.1 mm] High-precision North component of relative position vector */
-	int8_t      relPosHPE;       /**< [0.1 mm] High-precision East component of relative position vector */
-	int8_t      relPosHPD;       /**< [0.1 mm] High-precision Down component of relative position vector */
-	int8_t      relPosHPLength;  /**< [0.1 mm] High-precision component of the length of the relative position vector */
-	uint32_t    accN;            /**< [0.1 mm] Accuracy of relative position North component */
-	uint32_t    accE;            /**< [0.1 mm] Accuracy of relative position East component */
-	uint32_t    accD;            /**< [0.1 mm] Accuracy of relative position Down component */
-	uint32_t    accLength;       /**< [0.1 mm] Accuracy of the length of the relative position vector */
-	uint32_t    accHeading;      /**< [1e-5 deg] Accuracy of the heading of the relative position vector */
-	uint32_t    reserved2;
-	uint32_t    flags;
+    uint8_t  version;        /**< message version (expected 0x01) */
+    uint8_t  reserved0;
+    uint16_t refStationId;   /**< Reference station ID. Must be in the range 0..4095 */
+    uint32_t iTOW;           /**< [ms] GPS time of week of the navigation epoch */
+    int32_t  relPosN;        /**< [cm] North component of relative position vector */
+    int32_t  relPosE;        /**< [cm] East component of relative position vector */
+    int32_t  relPosD;        /**< [cm] Down component of relative position vector */
+    int32_t  relPosLength;   /**< [cm] Length of the relative position vector */
+    int32_t  relPosHeading;  /**< [1e-5 deg] Heading of the relative position vector */
+    uint32_t reserved1;
+    int8_t   relPosHPN;      /**< [0.1 mm] High-precision North component of relative position vector */
+    int8_t   relPosHPE;      /**< [0.1 mm] High-precision East component of relative position vector */
+    int8_t   relPosHPD;      /**< [0.1 mm] High-precision Down component of relative position vector */
+    int8_t   relPosHPLength; /**< [0.1 mm] High-precision component of the length of the relative position vector */
+    uint32_t accN;           /**< [0.1 mm] Accuracy of relative position North component */
+    uint32_t accE;           /**< [0.1 mm] Accuracy of relative position East component */
+    uint32_t accD;           /**< [0.1 mm] Accuracy of relative position Down component */
+    uint32_t accLength;      /**< [0.1 mm] Accuracy of the length of the relative position vector */
+    uint32_t accHeading;     /**< [1e-5 deg] Accuracy of the heading of the relative position vector */
+    uint32_t reserved2;
+    uint32_t flags;
 } ubx_payload_rx_nav_relposned_t;
 
 /* General message and payload buffer union */
 typedef union {
-	ubx_payload_rx_nav_pvt_t          payload_rx_nav_pvt;
-	ubx_payload_rx_nav_posllh_t       payload_rx_nav_posllh;
-	ubx_payload_rx_nav_sol_t          payload_rx_nav_sol;
-	ubx_payload_rx_nav_dop_t          payload_rx_nav_dop;
-	ubx_payload_rx_nav_timeutc_t      payload_rx_nav_timeutc;
-	ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
-	ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
-	ubx_payload_rx_nav_sat_part1_t    payload_rx_nav_sat_part1;
-	ubx_payload_rx_nav_sat_part2_t    payload_rx_nav_sat_part2;
-	ubx_payload_rx_nav_status_t       payload_rx_nav_status;
-	ubx_payload_rx_nav_svin_t         payload_rx_nav_svin;
-	ubx_payload_rx_nav_velned_t       payload_rx_nav_velned;
-	ubx_payload_rx_mon_hw_ubx6_t      payload_rx_mon_hw_ubx6;
-	ubx_payload_rx_mon_hw_ubx7_t      payload_rx_mon_hw_ubx7;
-	ubx_payload_rx_mon_rf_t           payload_rx_mon_rf;
-	ubx_payload_rx_mon_ver_part1_t    payload_rx_mon_ver_part1;
-	ubx_payload_rx_mon_ver_part2_t    payload_rx_mon_ver_part2;
-	ubx_payload_rx_ack_ack_t          payload_rx_ack_ack;
-	ubx_payload_rx_ack_nak_t          payload_rx_ack_nak;
-	ubx_payload_tx_cfg_prt_t          payload_tx_cfg_prt;
-	ubx_payload_tx_cfg_rate_t         payload_tx_cfg_rate;
-	ubx_payload_tx_cfg_nav5_t         payload_tx_cfg_nav5;
-	ubx_payload_tx_cfg_rst_t          payload_tx_cfg_rst;
-	ubx_payload_tx_cfg_sbas_t         payload_tx_cfg_sbas;
-	ubx_payload_tx_cfg_msg_t          payload_tx_cfg_msg;
-	ubx_payload_tx_cfg_tmode3_t       payload_tx_cfg_tmode3;
-	ubx_payload_tx_cfg_cfg_t          payload_tx_cfg_cfg;
-	ubx_payload_tx_cfg_valset_t       payload_tx_cfg_valset;
-	ubx_payload_tx_cfg_gnss_t         payload_tx_cfg_gnss;
-	ubx_payload_rx_nav_relposned_t    payload_rx_nav_relposned;
+    ubx_payload_rx_nav_pvt_t          payload_rx_nav_pvt;
+    ubx_payload_rx_nav_posllh_t       payload_rx_nav_posllh;
+    ubx_payload_rx_nav_sol_t          payload_rx_nav_sol;
+    ubx_payload_rx_nav_dop_t          payload_rx_nav_dop;
+    ubx_payload_rx_nav_timeutc_t      payload_rx_nav_timeutc;
+    ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
+    ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
+    ubx_payload_rx_nav_sat_part1_t    payload_rx_nav_sat_part1;
+    ubx_payload_rx_nav_sat_part2_t    payload_rx_nav_sat_part2;
+    ubx_payload_rx_nav_status_t       payload_rx_nav_status;
+    ubx_payload_rx_nav_svin_t         payload_rx_nav_svin;
+    ubx_payload_rx_nav_velned_t       payload_rx_nav_velned;
+    ubx_payload_rx_mon_hw_ubx6_t      payload_rx_mon_hw_ubx6;
+    ubx_payload_rx_mon_hw_ubx7_t      payload_rx_mon_hw_ubx7;
+    ubx_payload_rx_mon_rf_t           payload_rx_mon_rf;
+    ubx_payload_rx_mon_ver_part1_t    payload_rx_mon_ver_part1;
+    ubx_payload_rx_mon_ver_part2_t    payload_rx_mon_ver_part2;
+    ubx_payload_rx_ack_ack_t          payload_rx_ack_ack;
+    ubx_payload_rx_ack_nak_t          payload_rx_ack_nak;
+    ubx_payload_tx_cfg_prt_t          payload_tx_cfg_prt;
+    ubx_payload_tx_cfg_rate_t         payload_tx_cfg_rate;
+    ubx_payload_tx_cfg_nav5_t         payload_tx_cfg_nav5;
+    ubx_payload_tx_cfg_rst_t          payload_tx_cfg_rst;
+    ubx_payload_tx_cfg_sbas_t         payload_tx_cfg_sbas;
+    ubx_payload_tx_cfg_msg_t          payload_tx_cfg_msg;
+    ubx_payload_tx_cfg_tmode3_t       payload_tx_cfg_tmode3;
+    ubx_payload_tx_cfg_cfg_t          payload_tx_cfg_cfg;
+    ubx_payload_tx_cfg_valset_t       payload_tx_cfg_valset;
+    ubx_payload_tx_cfg_gnss_t         payload_tx_cfg_gnss;
+    ubx_payload_rx_nav_relposned_t    payload_rx_nav_relposned;
 } ubx_buf_t;
 
 #pragma pack(pop)
+
 /*** END OF u-blox protocol binary message and payload definitions ***/
 
 /* Decoder state */
 typedef enum {
-	UBX_DECODE_SYNC1 = 0,
-	UBX_DECODE_SYNC2,
-	UBX_DECODE_CLASS,
-	UBX_DECODE_ID,
-	UBX_DECODE_LENGTH1,
-	UBX_DECODE_LENGTH2,
-	UBX_DECODE_PAYLOAD,
-	UBX_DECODE_CHKSUM1,
-	UBX_DECODE_CHKSUM2,
-
-	UBX_DECODE_RTCM3
+    UBX_DECODE_SYNC1 = 0,
+    UBX_DECODE_SYNC2,
+    UBX_DECODE_CLASS,
+    UBX_DECODE_ID,
+    UBX_DECODE_LENGTH1,
+    UBX_DECODE_LENGTH2,
+    UBX_DECODE_PAYLOAD,
+    UBX_DECODE_CHKSUM1,
+    UBX_DECODE_CHKSUM2,
+
+    UBX_DECODE_RTCM3
 } ubx_decode_state_t;
 
 /* Rx message state */
 typedef enum {
-	UBX_RXMSG_IGNORE = 0,
-	UBX_RXMSG_HANDLE,
-	UBX_RXMSG_DISABLE,
-	UBX_RXMSG_ERROR_LENGTH
+    UBX_RXMSG_IGNORE = 0,
+    UBX_RXMSG_HANDLE,
+    UBX_RXMSG_DISABLE,
+    UBX_RXMSG_ERROR_LENGTH
 } ubx_rxmsg_state_t;
 
 /* ACK state */
 typedef enum {
-	UBX_ACK_IDLE = 0,
-	UBX_ACK_WAITING,
-	UBX_ACK_GOT_ACK,
-	UBX_ACK_GOT_NAK
+    UBX_ACK_IDLE = 0,
+    UBX_ACK_WAITING,
+    UBX_ACK_GOT_ACK,
+    UBX_ACK_GOT_NAK
 } ubx_ack_state_t;
 
-class GPSDriverUBX : public GPSBaseStationSupport
-{
+class GPSDriverUBX : public GPSBaseStationSupport {
 public:
-	enum class UBXMode : uint8_t {
-		Normal,                    ///< all non-heading configurations
-		RoverWithMovingBase,       ///< expect RTCM input on UART2 from a moving base for heading output
-		MovingBase,                ///< RTCM output on UART2 to a rover (GPS is installed on the vehicle)
-		RoverWithMovingBaseUART1, ///< expect RTCM input on UART1 from a moving base for heading output
-		MovingBaseUART1,          ///< RTCM output on UART1 to a rover (GPS is installed on the vehicle)
-		RoverWithStaticBaseUart2,  ///< expect RTCM input on UART2 from a static base.
-	};
-
-	GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,
-		     sensor_gps_s *gps_position, satellite_info_s *satellite_info,
-		     uint8_t dynamic_model = 7,
-		     float heading_offset = 0.f,
-		     int32_t uart2_baudrate = 57600,
-		     UBXMode mode = UBXMode::Normal);
-
-	virtual ~GPSDriverUBX();
-
-	int configure(unsigned &baudrate, const GPSConfig &config) override;
-	int receive(unsigned timeout) override;
-	int reset(GPSRestartType restart_type) override;
-
-	bool shouldInjectRTCM() override { return _mode != UBXMode::RoverWithMovingBase; }
-
-	enum class Board : uint8_t {
-		unknown = 0,
-		u_blox5 = 5,
-		u_blox6 = 6,
-		u_blox7 = 7,
-		u_blox8 = 8, ///< M8N or M8P
-		u_blox9 = 9, ///< M9N, or any F9*, except F9P
-		u_blox9_F9P = 10, ///< F9P
-		u_blox10 = 11,
-	};
-
-	const Board &board() const { return _board; }
+    enum class UBXMode : uint8_t {
+        Normal,                   ///< all non-heading configurations
+        RoverWithMovingBase,      ///< expect RTCM input on UART2 from a moving base for heading output
+        MovingBase,               ///< RTCM output on UART2 to a rover (GPS is installed on the vehicle)
+        RoverWithMovingBaseUART1, ///< expect RTCM input on UART1 from a moving base for heading output
+        MovingBaseUART1,          ///< RTCM output on UART1 to a rover (GPS is installed on the vehicle)
+        RoverWithStaticBaseUart2, ///< expect RTCM input on UART2 from a static base.
+    };
+
+    GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,
+                 sensor_gps_s *gps_position, satellite_info_s *satellite_info,
+                 uint8_t dynamic_model  = 7,
+                 float   heading_offset = 0.f,
+                 int32_t uart2_baudrate = 57600,
+                 UBXMode mode           = UBXMode::Normal);
+
+    virtual ~GPSDriverUBX();
+
+    int configure(unsigned &baudrate, const GPSConfig &config) override;
+    int receive(unsigned timeout) override;
+    int reset(GPSRestartType restart_type) override;
+
+    bool shouldInjectRTCM() override {
+        return _mode != UBXMode::RoverWithMovingBase;
+    }
+
+    enum class Board : uint8_t {
+        unknown     = 0,
+        u_blox5     = 5,
+        u_blox6     = 6,
+        u_blox7     = 7,
+        u_blox8     = 8,  ///< M8N or M8P
+        u_blox9     = 9,  ///< M9N, or any F9*, except F9P
+        u_blox9_F9P = 10, ///< F9P
+        u_blox10    = 11,
+    };
+
+    const Board &board() const {
+        return _board;
+    }
 
 private:
 
 private:
+    int activateRTCMOutput(bool reduce_update_rate);
 
-	int activateRTCMOutput(bool reduce_update_rate);
-
-	/**
+    /**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void addByteToChecksum(const uint8_t);
+    void addByteToChecksum(const uint8_t);
 
-	/**
+    /**
 	 * Calculate & add checksum for given buffer
 	 */
-	void calcChecksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
+    void calcChecksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
 
-	/**
+    /**
 	 * Configure message rate.
 	 * Note: this is deprecated with protocol version >= 27
 	 * @return true on success, false on write error
 	 */
-	bool configureMessageRate(const uint16_t msg, const uint8_t rate);
+    bool configureMessageRate(const uint16_t msg, const uint8_t rate);
 
-	/**
+    /**
 	 * Combines the configure_message_rate & wait_for_ack calls.
 	 * Note: this is deprecated with protocol version >= 27
 	 * @return true on success
 	 */
-	inline bool configureMessageRateAndAck(uint16_t msg, uint8_t rate, bool report_ack_error = false);
+    inline bool configureMessageRateAndAck(uint16_t msg, uint8_t rate, bool report_ack_error = false);
 
-	/**
+    /**
 	 * Send configuration values and desired message rates
 	 * @param config The configuration includes GNSS systems to use and protocol for interfaces
 	 * @param uart2_baudrate Baudrate of F9P's UART2 port
 	 * @return 0 on success, <0 on error
 	 */
-	int configureDevice(const GPSConfig &config, const int32_t uart2_baudrate);
-	/**
+    int configureDevice(const GPSConfig &config, const int32_t uart2_baudrate);
+    /**
 	 * Send configuration values and desired message rates (for protocol version < 27)
 	 * @param gnssSystems Set of GNSS systems to use
 	 * @return 0 on success, <0 on error
 	 */
-	int configureDevicePreV27(const GNSSSystemsMask &gnssSystems);
+    int configureDevicePreV27(const GNSSSystemsMask &gnssSystems);
 
-	/**
+    /**
 	 * Add a configuration value to _buf and increase the message size msg_size as needed
 	 * @param key_id one of the UBX_CFG_KEY_* constants
 	 * @param value configuration value
 	 * @param msg_size CFG-VALSET message size: this is an input & output param
 	 * @return true on success, false if buffer too small
 	 */
-	template<typename T>
-	bool cfgValset(uint32_t key_id, T value, int &msg_size);
+    template <typename T>
+    bool cfgValset(uint32_t key_id, T value, int &msg_size);
 
-	/**
+    /**
 	 * Add a configuration value that is port-specific (MSGOUT messages).
 	 * Note: Key ID must be the one for I2C, and the implementation assumes the
 	 *       Key ID's are in increasing order for the other ports: I2C, UART1, UART2, USB, SPI
@@ -1030,108 +1014,106 @@ class GPSDriverUBX : public GPSBaseStationSupport
 	 * @param msg_size CFG-VALSET message size: this is an input & output param
 	 * @return true on success, false if buffer too small
 	 */
-	bool cfgValsetPort(uint32_t key_id, uint8_t value, int &msg_size);
+    bool cfgValsetPort(uint32_t key_id, uint8_t value, int &msg_size);
 
-	/**
+    /**
 	 * Reset the parse state machine for a fresh start
 	 */
-	void decodeInit(void);
+    void decodeInit(void);
 
-	/**
+    /**
 	 * Calculate FNV1 hash
 	 */
-	uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
+    uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
 
-	/**
+    /**
 	 * Init _buf as CFG-VALSET
 	 * @return size of the message (without any config values)
 	 */
-	int initCfgValset();
+    int initCfgValset();
 
-	/**
+    /**
 	 * Start or restart the survey-in procees. This is only used in RTCM ouput mode.
 	 * It will be called automatically after configuring.
 	 * @return 0 on success, <0 on error
 	 */
-	int restartSurveyIn();
+    int restartSurveyIn();
 
-	/**
+    /**
 	 * restartSurveyIn for protocol version < 27 (_proto_ver_27_or_higher == false)
 	 */
-	int restartSurveyInPreV27();
+    int restartSurveyInPreV27();
 
-	/**
+    /**
 	 * Parse the binary UBX packet
 	 */
-	int parseChar(const uint8_t b);
+    int parseChar(const uint8_t b);
 
-	/**
+    /**
 	 * Start payload rx
 	 */
-	int payloadRxInit(void);
+    int payloadRxInit(void);
 
-	/**
+    /**
 	 * Add payload rx byte
 	 */
-	int payloadRxAdd(const uint8_t b);
-	int payloadRxAddMonVer(const uint8_t b);
-	int payloadRxAddNavSat(const uint8_t b);
-	int payloadRxAddNavSvinfo(const uint8_t b);
+    int payloadRxAdd(const uint8_t b);
+    int payloadRxAddMonVer(const uint8_t b);
+    int payloadRxAddNavSat(const uint8_t b);
+    int payloadRxAddNavSvinfo(const uint8_t b);
 
-	/**
+    /**
 	 * Finish payload rx
 	 */
-	int payloadRxDone(void);
+    int payloadRxDone(void);
 
-	/**
+    /**
 	 * Send a message
 	 * @return true on success, false on write error (errno set)
 	 */
-	bool sendMessage(const uint16_t msg, const uint8_t *payload, const uint16_t length);
+    bool sendMessage(const uint16_t msg, const uint8_t *payload, const uint16_t length);
 
-	/**
+    /**
 	 * Wait for message acknowledge
 	 */
-	int waitForAck(const uint16_t msg, const unsigned timeout, const bool report);
+    int waitForAck(const uint16_t msg, const unsigned timeout, const bool report);
 
-	const Interface _interface{};
+    const Interface _interface{};
 
-	gps_abstime             _disable_cmd_last{0};
-	sensor_gps_s           *_gps_position {nullptr};
-	satellite_info_s       *_satellite_info {nullptr};
-	ubx_ack_state_t         _ack_state{UBX_ACK_IDLE};
-	ubx_buf_t               _buf{};
-	ubx_decode_state_t      _decode_state{};
-	ubx_rxmsg_state_t       _rx_state{UBX_RXMSG_IGNORE};
+    gps_abstime        _disable_cmd_last{0};
+    sensor_gps_s      *_gps_position{nullptr};
+    satellite_info_s  *_satellite_info{nullptr};
+    ubx_ack_state_t    _ack_state{UBX_ACK_IDLE};
+    ubx_buf_t          _buf{};
+    ubx_decode_state_t _decode_state{};
+    ubx_rxmsg_state_t  _rx_state{UBX_RXMSG_IGNORE};
 
-	bool _configured{false};
-	bool _got_posllh{false};
-	bool _got_velned{false};
-	bool _proto_ver_27_or_higher{false}; ///< true if protocol version 27 or higher detected
-	bool _use_nav_pvt{false};
+    bool _configured{false};
+    bool _got_posllh{false};
+    bool _got_velned{false};
+    bool _proto_ver_27_or_higher{false}; ///< true if protocol version 27 or higher detected
+    bool _use_nav_pvt{false};
 
-	uint8_t _rx_ck_a{0};
-	uint8_t _rx_ck_b{0};
-	uint8_t _dyn_model{7};  ///< ublox Dynamic platform model default 7: airborne with <2g acceleration
+    uint8_t _rx_ck_a{0};
+    uint8_t _rx_ck_b{0};
+    uint8_t _dyn_model{7}; ///< ublox Dynamic platform model default 7: airborne with <2g acceleration
 
-	uint16_t _ack_waiting_msg{0};
-	uint16_t _rx_msg{};
-	uint16_t _rx_payload_index{0};
-	uint16_t _rx_payload_length{0};
+    uint16_t _ack_waiting_msg{0};
+    uint16_t _rx_msg{};
+    uint16_t _rx_payload_index{0};
+    uint16_t _rx_payload_length{0};
 
-	uint32_t _ubx_version{0};
+    uint32_t _ubx_version{0};
 
-	uint64_t _last_timestamp_time{0};
+    uint64_t _last_timestamp_time{0};
 
-	Board _board{Board::unknown};
+    Board _board{Board::unknown};
 
-	OutputMode _output_mode{OutputMode::GPS};
+    OutputMode _output_mode{OutputMode::GPS};
 
-	RTCMParsing *_rtcm_parsing{nullptr};
+    RTCMParsing *_rtcm_parsing{nullptr};
 
-	const UBXMode _mode;
-	const float _heading_offset;
-	const int32_t _uart2_baudrate;
+    const UBXMode _mode;
+    const float   _heading_offset;
+    const int32_t _uart2_baudrate;
 };
-
-
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/unicore.cpp b/apps/peripheral/sensor/gnss/gps/devices/src/unicore.cpp
index 50f4e31e5d..99f8934ca1 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/unicore.cpp
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/unicore.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "unicore.h"
 #include "crc.h"
@@ -37,146 +14,138 @@
 #include <stdlib.h>
 #include <string.h>
 
-UnicoreParser::Result UnicoreParser::parseChar(char c)
-{
-	switch (_state) {
-	case State::Uninit:
-		if (c == '#') {
-			_state = State::GotHashtag;
-		}
-
-		break;
+UnicoreParser::Result UnicoreParser::parseChar(char c) {
+    switch (_state) {
+    case State::Uninit:
+        if (c == '#') {
+            _state = State::GotHashtag;
+        }
 
-	case State::GotHashtag:
-		if (c == '*') {
-			_state = State::GotStar;
+        break;
 
-			// Make sure buffer is zero terminated.
-			_buffer[_buffer_pos] = '\0';
+    case State::GotHashtag:
+        if (c == '*') {
+            _state = State::GotStar;
 
-		} else {
-			if (_buffer_pos >= sizeof(_buffer) - 1) {
-				reset();
-				return Result::None;
-			}
+            // Make sure buffer is zero terminated.
+            _buffer[_buffer_pos] = '\0';
 
-			_buffer[_buffer_pos++] = c;
-		}
+        } else {
+            if (_buffer_pos >= sizeof(_buffer) - 1) {
+                reset();
+                return Result::None;
+            }
 
-		break;
+            _buffer[_buffer_pos++] = c;
+        }
 
-	case State::GotStar:
-		_buffer_crc[_buffer_crc_pos++] = c;
+        break;
 
-		if (_buffer_crc_pos >= 8) {
+    case State::GotStar:
+        _buffer_crc[_buffer_crc_pos++] = c;
 
-			// Make sure the CRC buffer is zero terminated.
-			_buffer_crc[_buffer_crc_pos] = '\0';
+        if (_buffer_crc_pos >= 8) {
+            // Make sure the CRC buffer is zero terminated.
+            _buffer_crc[_buffer_crc_pos] = '\0';
 
-			if (!crcCorrect()) {
-				return Result::WrongCrc;
-			}
+            if (!crcCorrect()) {
+                return Result::WrongCrc;
+            }
 
-			if (isHeading()) {
-				if (extractHeading()) {
-					reset();
-					return Result::GotHeading;
+            if (isHeading()) {
+                if (extractHeading()) {
+                    reset();
+                    return Result::GotHeading;
 
-				} else {
-					reset();
-					return Result::WrongStructure;
-				}
+                } else {
+                    reset();
+                    return Result::WrongStructure;
+                }
 
-			} else if (isAgrica()) {
-				if (extractAgrica()) {
-					reset();
-					return Result::GotAgrica;
+            } else if (isAgrica()) {
+                if (extractAgrica()) {
+                    reset();
+                    return Result::GotAgrica;
 
-				} else {
-					reset();
-					return Result::WrongStructure;
-				}
+                } else {
+                    reset();
+                    return Result::WrongStructure;
+                }
 
-			} else {
-				reset();
-				return Result::UnknownSentence;
-			}
-		}
+            } else {
+                reset();
+                return Result::UnknownSentence;
+            }
+        }
 
-		break;
-	}
+        break;
+    }
 
-	return Result::None;
+    return Result::None;
 }
 
-void UnicoreParser::reset()
-{
-	_state = State::Uninit;
-	_buffer_pos = 0;
-	_buffer_crc_pos = 0;
+void UnicoreParser::reset() {
+    _state          = State::Uninit;
+    _buffer_pos     = 0;
+    _buffer_crc_pos = 0;
 }
 
-bool UnicoreParser::crcCorrect() const
-{
-	const uint32_t crc_calculated = calculateCRC32(_buffer_pos, (uint8_t *)_buffer, 0);
-	const uint32_t crc_parsed = (uint32_t)strtoul(_buffer_crc, nullptr, 16);
-	return (crc_calculated == crc_parsed);
+bool UnicoreParser::crcCorrect() const {
+    const uint32_t crc_calculated = calculateCRC32(_buffer_pos, (uint8_t *)_buffer, 0);
+    const uint32_t crc_parsed     = (uint32_t)strtoul(_buffer_crc, nullptr, 16);
+    return (crc_calculated == crc_parsed);
 }
 
-bool UnicoreParser::isHeading() const
-{
-	const char header[] = "UNIHEADINGA";
+bool UnicoreParser::isHeading() const {
+    const char header[] = "UNIHEADINGA";
 
-	return strncmp(header, _buffer, strlen(header)) == 0;
+    return strncmp(header, _buffer, strlen(header)) == 0;
 }
 
-bool UnicoreParser::isAgrica() const
-{
-	const char header[] = "AGRICA";
+bool UnicoreParser::isAgrica() const {
+    const char header[] = "AGRICA";
 
-	return strncmp(header, _buffer, strlen(header)) == 0;
+    return strncmp(header, _buffer, strlen(header)) == 0;
 }
 
-bool UnicoreParser::extractHeading()
-{
-	// The basline starts after ;,, and then follows the heading.
+bool UnicoreParser::extractHeading() {
+    // The basline starts after ;,, and then follows the heading.
 
-	// Skip to ;
-	char *ptr = strchr(_buffer, ';');
+    // Skip to ;
+    char *ptr = strchr(_buffer, ';');
 
-	if (ptr == nullptr) {
-		return false;
-	}
+    if (ptr == nullptr) {
+        return false;
+    }
 
-	ptr = strtok(ptr, ",");
+    ptr = strtok(ptr, ",");
 
-	unsigned i = 0;
+    unsigned i = 0;
 
-	while (ptr != nullptr) {
-		ptr = strtok(nullptr, ",");
+    while (ptr != nullptr) {
+        ptr = strtok(nullptr, ",");
 
-		if (ptr == nullptr) {
-			return false;
-		}
+        if (ptr == nullptr) {
+            return false;
+        }
 
-		if (i == 1) {
-			_heading.baseline_m = strtof(ptr, nullptr);
+        if (i == 1) {
+            _heading.baseline_m = strtof(ptr, nullptr);
 
-		} else if (i == 2) {
-			_heading.heading_deg = strtof(ptr, nullptr);
+        } else if (i == 2) {
+            _heading.heading_deg = strtof(ptr, nullptr);
 
-		} else if (i == 5) {
-			_heading.heading_stddev_deg = strtof(ptr, nullptr);
-			return true;
-		}
+        } else if (i == 5) {
+            _heading.heading_stddev_deg = strtof(ptr, nullptr);
+            return true;
+        }
 
-		++i;
-	}
+        ++i;
+    }
 
-	return false;
+    return false;
 }
 
-bool UnicoreParser::extractAgrica()
-{
-	return true;
+bool UnicoreParser::extractAgrica() {
+    return true;
 }
diff --git a/apps/peripheral/sensor/gnss/gps/devices/src/unicore.h b/apps/peripheral/sensor/gnss/gps/devices/src/unicore.h
index 4433aaaeb5..477fd0e630 100644
--- a/apps/peripheral/sensor/gnss/gps/devices/src/unicore.h
+++ b/apps/peripheral/sensor/gnss/gps/devices/src/unicore.h
@@ -1,95 +1,68 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
 #include <cstdint>
 
-
-class UnicoreParser
-{
+class UnicoreParser {
 public:
-	enum class Result {
-		None,
-		WrongCrc,
-		WrongStructure,
-		GotHeading,
-		GotAgrica,
-		UnknownSentence,
-	};
+    enum class Result {
+        None,
+        WrongCrc,
+        WrongStructure,
+        GotHeading,
+        GotAgrica,
+        UnknownSentence,
+    };
 
-	Result parseChar(char c);
+    Result parseChar(char c);
 
-	struct Heading {
-		float heading_deg;
-		float heading_stddev_deg;
-		float baseline_m;
-	};
+    struct Heading {
+        float heading_deg;
+        float heading_stddev_deg;
+        float baseline_m;
+    };
 
-	struct Agrica {
-	};
+    struct Agrica {
+    };
 
-	Heading heading() const
-	{
-		return _heading;
-	}
+    Heading heading() const {
+        return _heading;
+    }
 
-	Agrica agrica() const
-	{
-		return _agrica;
-	}
+    Agrica agrica() const {
+        return _agrica;
+    }
 
 
 private:
-	void reset();
-	bool crcCorrect() const;
-	bool isHeading() const;
-	bool isAgrica() const;
-	bool extractHeading();
-	bool extractAgrica();
+    void reset();
+    bool crcCorrect() const;
+    bool isHeading() const;
+    bool isAgrica() const;
+    bool extractHeading();
+    bool extractAgrica();
 
-	// We have seen buffers with 540 bytes for AGRICA.
-	char _buffer[600];
-	unsigned _buffer_pos {0};
-	char _buffer_crc[9];
-	unsigned _buffer_crc_pos {0};
+    // We have seen buffers with 540 bytes for AGRICA.
+    char     _buffer[600];
+    unsigned _buffer_pos{0};
+    char     _buffer_crc[9];
+    unsigned _buffer_crc_pos{0};
 
-	enum class State {
-		Uninit,
-		GotHashtag,
-		GotStar,
-	} _state {State::Uninit};
+    enum class State {
+        Uninit,
+        GotHashtag,
+        GotStar,
+    } _state{State::Uninit};
 
-	Heading _heading{};
-	Agrica _agrica{};
+    Heading _heading{};
+    Agrica  _agrica{};
 };
diff --git a/apps/peripheral/sensor/gnss/gps/gps.cpp b/apps/peripheral/sensor/gnss/gps/gps.cpp
index 4ed29a8c6e..2ecb924aa7 100644
--- a/apps/peripheral/sensor/gnss/gps/gps.cpp
+++ b/apps/peripheral/sensor/gnss/gps/gps.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file gps.cpp
diff --git a/apps/peripheral/sensor/gnss/gps/params.c b/apps/peripheral/sensor/gnss/gps/params.c
index 49b4da7065..5b0a83d415 100644
--- a/apps/peripheral/sensor/gnss/gps/params.c
+++ b/apps/peripheral/sensor/gnss/gps/params.c
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016 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.
- *
- ****************************************************************************/
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * Log GPS communication data
diff --git a/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.cpp b/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.cpp
index 399f034fbf..46ed5ab9d1 100644
--- a/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.cpp
+++ b/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file PPSCapture.cpp
@@ -46,183 +23,173 @@
 #include <systemlib/mavlink_log.h>
 
 PPSCapture::PPSCapture() :
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-	_pps_capture_pub.advertise();
+    ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {
+    _pps_capture_pub.advertise();
 }
 
-PPSCapture::~PPSCapture()
-{
-	if (_channel >= 0) {
-		io_timer_unallocate_channel(_channel);
-		px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr);
-	}
+PPSCapture::~PPSCapture() {
+    if (_channel >= 0) {
+        io_timer_unallocate_channel(_channel);
+        px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr);
+    }
 }
 
-bool PPSCapture::init()
-{
-	bool success = false;
+bool PPSCapture::init() {
+    bool success = false;
 
-	for (unsigned i = 0; i < 16; ++i) {
-		char param_name[17];
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
-		param_t function_handle = param_find(param_name);
-		int32_t function;
+    for (unsigned i = 0; i < 16; ++i) {
+        char param_name[17];
+        snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+        param_t function_handle = param_find(param_name);
+        int32_t function;
 
-		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
-			if (function == 2064) { // PPS_Input
-				_channel = i;
-			}
-		}
-	}
+        if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+            if (function == 2064) { // PPS_Input
+                _channel = i;
+            }
+        }
+    }
 
 #if defined(PPS_CAPTURE_CHANNEL)
 
-	if (_channel == -1) {
-		_channel = PPS_CAPTURE_CHANNEL;
-	}
+    if (_channel == -1) {
+        _channel = PPS_CAPTURE_CHANNEL;
+    }
 
 #endif
 
-	if (_channel == -1) {
-		PX4_WARN("No pps channel configured");
-		return false;
-	}
+    if (_channel == -1) {
+        PX4_WARN("No pps channel configured");
+        return false;
+    }
 
-	int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_PPS);
+    int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_PPS);
 
-	if (ret != PX4_OK) {
-		PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, _channel);
-		return false;
-	}
+    if (ret != PX4_OK) {
+        PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, _channel);
+        return false;
+    }
 
-	_pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel));
-	int ret_val = px4_arch_gpiosetevent(_pps_capture_gpio, true, false, true, &PPSCapture::gpio_interrupt_callback, this);
+    _pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel));
+    int ret_val       = px4_arch_gpiosetevent(_pps_capture_gpio, true, false, true, &PPSCapture::gpio_interrupt_callback, this);
 
-	if (ret_val == PX4_OK) {
-		success = true;
-	}
+    if (ret_val == PX4_OK) {
+        success = true;
+    }
 
-	return success;
+    return success;
 }
 
-void PPSCapture::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
-
-	sensor_gps_s sensor_gps;
-
-	if (_sensor_gps_sub.update(&sensor_gps)) {
-		_last_gps_utc_timestamp = sensor_gps.time_utc_usec;
-		_last_gps_timestamp = sensor_gps.timestamp;
-	}
-
-	pps_capture_s pps_capture;
-	pps_capture.timestamp = _hrt_timestamp;
-	pps_capture.pps_rate_exceeded_counter = _pps_rate_exceeded_counter;
-	// GPS UTC time when the GPIO interrupt was triggered
-	// Last UTC time received from the GPS + elapsed time to the PPS interrupt
-	uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp);
-
-	// (For ubx F9P) The rising edge of the PPS pulse is aligned to the top of second GPS time base.
-	// So, remove the fraction of second and shift to the next second. The interrupt is triggered
-	// before the matching timestamp is received via a UART message, which means the last received GPS time is always
-	// behind.
-	pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC;
-
-	_pps_capture_pub.publish(pps_capture);
-
-	if (_pps_rate_failure.load()) {
-		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: GPS PPS disabled\t");
-		events::send(events::ID("pps_capture_pps_rate_exceeded"),
-			     events::Log::Error, "Hardware fault: GPS PPS disabled");
-		_pps_rate_failure.store(false);
-	}
+void PPSCapture::Run() {
+    if (should_exit()) {
+        exit_and_cleanup();
+        return;
+    }
+
+    sensor_gps_s sensor_gps;
+
+    if (_sensor_gps_sub.update(&sensor_gps)) {
+        _last_gps_utc_timestamp = sensor_gps.time_utc_usec;
+        _last_gps_timestamp     = sensor_gps.timestamp;
+    }
+
+    pps_capture_s pps_capture;
+    pps_capture.timestamp                 = _hrt_timestamp;
+    pps_capture.pps_rate_exceeded_counter = _pps_rate_exceeded_counter;
+    // GPS UTC time when the GPIO interrupt was triggered
+    // Last UTC time received from the GPS + elapsed time to the PPS interrupt
+    uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp);
+
+    // (For ubx F9P) The rising edge of the PPS pulse is aligned to the top of second GPS time base.
+    // So, remove the fraction of second and shift to the next second. The interrupt is triggered
+    // before the matching timestamp is received via a UART message, which means the last received GPS time is always
+    // behind.
+    pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC;
+
+    _pps_capture_pub.publish(pps_capture);
+
+    if (_pps_rate_failure.load()) {
+        mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: GPS PPS disabled\t");
+        events::send(events::ID("pps_capture_pps_rate_exceeded"),
+                     events::Log::Error, "Hardware fault: GPS PPS disabled");
+        _pps_rate_failure.store(false);
+    }
 }
 
-int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
-{
-	PPSCapture *instance = static_cast<PPSCapture *>(arg);
+int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg) {
+    PPSCapture *instance = static_cast<PPSCapture *>(arg);
 
-	hrt_abstime interrupt_time = hrt_absolute_time();
+    hrt_abstime interrupt_time = hrt_absolute_time();
 
-	if ((interrupt_time - instance->_hrt_timestamp) < 50_ms) {
-		++(instance->_pps_rate_exceeded_counter);
+    if ((interrupt_time - instance->_hrt_timestamp) < 50_ms) {
+        ++(instance->_pps_rate_exceeded_counter);
 
-		if (instance->_pps_rate_exceeded_counter >= 10) {
-			// Trigger rate too high, stop future interrupts
-			px4_arch_gpiosetevent(instance->_pps_capture_gpio, false, false, false, nullptr, nullptr);
-			instance->_pps_rate_failure.store(true);
-		}
-	}
+        if (instance->_pps_rate_exceeded_counter >= 10) {
+            // Trigger rate too high, stop future interrupts
+            px4_arch_gpiosetevent(instance->_pps_capture_gpio, false, false, false, nullptr, nullptr);
+            instance->_pps_rate_failure.store(true);
+        }
+    }
 
-	instance->_hrt_timestamp = interrupt_time;
-	instance->ScheduleNow(); // schedule work queue to publish PPS captured time
+    instance->_hrt_timestamp = interrupt_time;
+    instance->ScheduleNow(); // schedule work queue to publish PPS captured time
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-int PPSCapture::task_spawn(int argc, char *argv[])
-{
-	PPSCapture *instance = new PPSCapture();
+int PPSCapture::task_spawn(int argc, char *argv[]) {
+    PPSCapture *instance = new PPSCapture();
 
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
+    if (instance) {
+        _object.store(instance);
+        _task_id = task_id_is_work_queue;
 
-		if (instance->init()) {
-			return PX4_OK;
-		}
+        if (instance->init()) {
+            return PX4_OK;
+        }
 
-	} else {
-		PX4_ERR("alloc failed");
-	}
+    } else {
+        PX4_ERR("alloc failed");
+    }
 
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
+    delete instance;
+    _object.store(nullptr);
+    _task_id = -1;
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-int PPSCapture::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
+int PPSCapture::custom_command(int argc, char *argv[]) {
+    return print_usage("unknown command");
 }
 
-int PPSCapture::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
+int PPSCapture::print_usage(const char *reason) {
+    if (reason) {
+        PX4_WARN("%s\n", reason);
+    }
 
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
+    PRINT_MODULE_DESCRIPTION(
+        R"DESCR_STR(
 ### Description
 This implements capturing PPS information from the GNSS module and calculates the drift between PPS and Real-time clock.
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("pps_capture", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    PRINT_MODULE_USAGE_NAME("pps_capture", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
-	return 0;
+    return 0;
 }
 
-void PPSCapture::stop()
-{
-	exit_and_cleanup();
+void PPSCapture::stop() {
+    exit_and_cleanup();
 }
 
-extern "C" __EXPORT int pps_capture_main(int argc, char *argv[])
-{
-	if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running()) {
-		PPSCapture::stop();
-	}
+extern "C" __EXPORT int pps_capture_main(int argc, char *argv[]) {
+    if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running()) {
+        PPSCapture::stop();
+    }
 
-	return PPSCapture::main(argc, argv);
+    return PPSCapture::main(argc, argv);
 }
diff --git a/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.hpp b/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.hpp
index 48a0774495..9150ac64df 100644
--- a/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.hpp
+++ b/apps/peripheral/sensor/gnss/pps_capture/PPSCapture.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -44,41 +21,40 @@
 
 using namespace time_literals;
 
-class PPSCapture : public ModuleBase<PPSCapture>, public px4::ScheduledWorkItem
-{
+class PPSCapture : public ModuleBase<PPSCapture>, public px4::ScheduledWorkItem {
 public:
-	PPSCapture();
-	virtual ~PPSCapture();
+    PPSCapture();
+    virtual ~PPSCapture();
 
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
+    /** @see ModuleBase */
+    static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
+    /** @see ModuleBase */
+    static int custom_command(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
+    /** @see ModuleBase */
+    static int print_usage(const char *reason = nullptr);
 
-	bool init();
+    bool init();
 
-	static int gpio_interrupt_callback(int irq, void *context, void *arg);
+    static int gpio_interrupt_callback(int irq, void *context, void *arg);
 
-	/** PPSCapture is an interrupt-driven task and needs to be manually stopped */
-	static void stop();
+    /** PPSCapture is an interrupt-driven task and needs to be manually stopped */
+    static void stop();
 
 private:
-	void Run() override;
+    void Run() override;
 
-	int _channel{-1};
-	uint32_t _pps_capture_gpio{0};
-	uORB::Publication<pps_capture_s>	_pps_capture_pub{ORB_ID(pps_capture)};
-	uORB::Subscription								_sensor_gps_sub{ORB_ID(sensor_gps)};
-	orb_advert_t											_mavlink_log_pub{nullptr};
+    int                              _channel{-1};
+    uint32_t                         _pps_capture_gpio{0};
+    uORB::Publication<pps_capture_s> _pps_capture_pub{ORB_ID(pps_capture)};
+    uORB::Subscription               _sensor_gps_sub{ORB_ID(sensor_gps)};
+    orb_advert_t                     _mavlink_log_pub{nullptr};
 
-	hrt_abstime _hrt_timestamp{0};
+    hrt_abstime _hrt_timestamp{0};
 
-	hrt_abstime	_last_gps_timestamp{0};
-	uint64_t		_last_gps_utc_timestamp{0};
-	uint8_t			_pps_rate_exceeded_counter{0};
-	px4::atomic<bool>			_pps_rate_failure{false};
+    hrt_abstime       _last_gps_timestamp{0};
+    uint64_t          _last_gps_utc_timestamp{0};
+    uint8_t           _pps_rate_exceeded_counter{0};
+    px4::atomic<bool> _pps_rate_failure{false};
 };
diff --git a/apps/peripheral/sensor/gnss/pps_capture/pps_capture_params.c b/apps/peripheral/sensor/gnss/pps_capture/pps_capture_params.c
index 68442ec839..9f801d3d52 100644
--- a/apps/peripheral/sensor/gnss/pps_capture/pps_capture_params.c
+++ b/apps/peripheral/sensor/gnss/pps_capture/pps_capture_params.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file pps_capture_params.c
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055.cpp b/apps/peripheral/sensor/imu/bmi055/BMI055.cpp
index dc11e1b96b..54c40ed41d 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055.cpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055.cpp
@@ -1,88 +1,62 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
 
 #include "BMI055.hpp"
 
 #include "BMI055_Accelerometer.hpp"
 #include "BMI055_Gyroscope.hpp"
 
-I2CSPIDriverBase *BMI055::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	BMI055 *instance = nullptr;
+I2CSPIDriverBase *BMI055::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    BMI055 *instance = nullptr;
 
-	if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI055) {
-		instance = new Bosch::BMI055::Accelerometer::BMI055_Accelerometer(config);
+    if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI055) {
+        instance = new Bosch::BMI055::Accelerometer::BMI055_Accelerometer(config);
 
-	} else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI055) {
-		instance = new Bosch::BMI055::Gyroscope::BMI055_Gyroscope(config);
-	}
+    } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI055) {
+        instance = new Bosch::BMI055::Gyroscope::BMI055_Gyroscope(config);
+    }
 
-	if (!instance) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
+    if (!instance) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
 
-	if (OK != instance->init()) {
-		delete instance;
-		return nullptr;
-	}
+    if (OK != instance->init()) {
+        delete instance;
+        return nullptr;
+    }
 
-	return instance;
+    return instance;
 }
 
 BMI055::BMI055(const I2CSPIDriverConfig &config) :
-	SPI(config),
-	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio)
-{
+    SPI(config),
+    I2CSPIDriver(config),
+    _drdy_gpio(config.drdy_gpio) {
 }
 
-int BMI055::init()
-{
-	int ret = SPI::init();
+int BMI055::init() {
+    int ret = SPI::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("SPI::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool BMI055::Reset()
-{
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool BMI055::Reset() {
+    _state = STATE::RESET;
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055.hpp b/apps/peripheral/sensor/imu/bmi055/BMI055.hpp
index f4fad2772e..82c445a315 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055.hpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055.hpp
@@ -1,35 +1,16 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#ifndef __BMI055_H__
+#define __BMI055_H__
+
 
 #pragma once
 
@@ -38,44 +19,45 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
+}
 
-class BMI055 : public device::SPI, public I2CSPIDriver<BMI055>
-{
+class BMI055 : public device::SPI, public I2CSPIDriver<BMI055> {
 public:
-	BMI055(const I2CSPIDriverConfig &config);
+    BMI055(const I2CSPIDriverConfig &config);
 
-	virtual ~BMI055() = default;
+    virtual ~BMI055() = default;
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	virtual void RunImpl() = 0;
+    virtual void RunImpl() = 0;
 
-	int init() override;
-	virtual void print_status() = 0;
+    int          init() override;
+    virtual void print_status() = 0;
 
 protected:
+    bool Reset();
 
-	bool Reset();
-
-	const spi_drdy_gpio_t _drdy_gpio;
+    const spi_drdy_gpio_t _drdy_gpio;
 
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_config_check_timestamp{0};
+    hrt_abstime _temperature_update_timestamp{0};
+    int         _failure_count{0};
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _data_ready_interrupt_enabled{false};
+    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                     _data_ready_interrupt_enabled{false};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_READ,
-	} _state{STATE::RESET};
-
-	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        FIFO_READ,
+    } _state{STATE::RESET};
 
+    uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 };
+
+#endif // __BMI055_H__
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.cpp b/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.cpp
index 77cf537302..0a05eac708 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.cpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMI055_Accelerometer.hpp"
 
@@ -37,446 +14,422 @@
 
 using namespace time_literals;
 
-namespace Bosch::BMI055::Accelerometer
-{
+namespace Bosch::BMI055::Accelerometer {
 
 BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) :
-	BMI055(config),
-	_px4_accel(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
-	}
-
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
-}
+    BMI055(config),
+    _px4_accel(get_device_id(), config.rotation) {
+    if (config.drdy_gpio != 0) {
+        _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME "_accel: DRDY missed");
+    }
 
-BMI055_Accelerometer::~BMI055_Accelerometer()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
+    ConfigureSampleRate(_px4_accel.get_max_rate_hz());
 }
 
-void BMI055_Accelerometer::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+BMI055_Accelerometer::~BMI055_Accelerometer() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_fifo_empty_perf);
+    perf_free(_fifo_overflow_perf);
+    perf_free(_fifo_reset_perf);
+    perf_free(_drdy_missed_perf);
 }
 
-void BMI055_Accelerometer::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
+void BMI055_Accelerometer::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-int BMI055_Accelerometer::probe()
-{
-	const uint8_t BGW_CHIPID = RegisterRead(Register::BGW_CHIPID);
+void BMI055_Accelerometer::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	if (BGW_CHIPID != chip_id) {
-		DEVICE_DEBUG("unexpected BGW_CHIPID 0x%02x", BGW_CHIPID);
-		return PX4_ERROR;
-	}
+    PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
 
-	return PX4_OK;
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_fifo_empty_perf);
+    perf_print_counter(_fifo_overflow_perf);
+    perf_print_counter(_fifo_reset_perf);
+    perf_print_counter(_drdy_missed_perf);
 }
 
-void BMI055_Accelerometer::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
-		RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(25_ms);
-		break;
+int BMI055_Accelerometer::probe() {
+    const uint8_t BGW_CHIPID = RegisterRead(Register::BGW_CHIPID);
 
-	case STATE::WAIT_FOR_RESET:
-		if (RegisterRead(Register::BGW_CHIPID) == chip_id) {
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(25_ms);
+    if (BGW_CHIPID != chip_id) {
+        DEVICE_DEBUG("unexpected BGW_CHIPID 0x%02x", BGW_CHIPID);
+        return PX4_ERROR;
+    }
 
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start reading from FIFO
-			_state = STATE::FIFO_READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = 0;
-
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			// always check current FIFO status/count
-			bool success = false;
-			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
-
-			if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
-				FIFOReset();
-				perf_count(_fifo_overflow_perf);
-
-			} else {
-				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
-
-				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
-					// not technically an overflow, but more samples than we expected or can publish
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_frame_counter == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else if (fifo_frame_counter >= 1) {
-
-					uint8_t samples = fifo_frame_counter;
-
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
-					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
-						success = true;
-
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
+    return PX4_OK;
+}
 
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-
-			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
-			}
-		}
-
-		break;
-	}
+void BMI055_Accelerometer::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
+        RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        ScheduleDelayed(25_ms);
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+        if (RegisterRead(Register::BGW_CHIPID) == chip_id) {
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(25_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start reading from FIFO
+            _state = STATE::FIFO_READ;
+
+            if (DataReadyInterruptConfigure()) {
+                _data_ready_interrupt_enabled = true;
+
+                // backup schedule as a watchdog timeout
+                ScheduleDelayed(100_ms);
+
+            } else {
+                _data_ready_interrupt_enabled = false;
+                ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+            }
+
+            FIFOReset();
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::FIFO_READ: {
+        hrt_abstime timestamp_sample = 0;
+
+        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) < _fifo_empty_interval_us) {
+                timestamp_sample = drdy_timestamp_sample;
+
+            } else {
+                perf_count(_drdy_missed_perf);
+            }
+
+            // push backup schedule back
+            ScheduleDelayed(_fifo_empty_interval_us * 2);
+        }
+
+        // always check current FIFO status/count
+        bool          success     = false;
+        const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+
+        if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
+            FIFOReset();
+            perf_count(_fifo_overflow_perf);
+
+        } else {
+            const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
+
+            if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+                // not technically an overflow, but more samples than we expected or can publish
+                FIFOReset();
+                perf_count(_fifo_overflow_perf);
+
+            } else if (fifo_frame_counter == 0) {
+                perf_count(_fifo_empty_perf);
+
+            } else if (fifo_frame_counter >= 1) {
+                uint8_t samples = fifo_frame_counter;
+
+                // tolerate minor jitter, leave sample to next iteration if behind by only 1
+                if (samples == _fifo_samples + 1) {
+                    // sample timestamp set from data ready already corresponds to _fifo_samples
+                    if (timestamp_sample == 0) {
+                        timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+                    }
+
+                    samples--;
+                }
+
+                if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
+                    success = true;
+
+                    if (_failure_count > 0) {
+                        _failure_count--;
+                    }
+                }
+            }
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+
+        } else {
+            // periodically update temperature (~1 Hz)
+            if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+                UpdateTemperature();
+                _temperature_update_timestamp = now;
+            }
+        }
+    }
+
+    break;
+    }
 }
 
-void BMI055_Accelerometer::ConfigureAccel()
-{
-	const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
-
-	switch (PMU_RANGE) {
-	case range_2g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case range_4g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case range_8g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case range_16g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
+void BMI055_Accelerometer::ConfigureAccel() {
+    const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
+
+    switch (PMU_RANGE) {
+    case range_2g_set:
+        _px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
+        _px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+        break;
+
+    case range_4g_set:
+        _px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
+        _px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+        break;
+
+    case range_8g_set:
+        _px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
+        _px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+        break;
+
+    case range_16g_set:
+        _px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
+        _px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+        break;
+    }
 }
 
-void BMI055_Accelerometer::ConfigureSampleRate(int sample_rate)
-{
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+void BMI055_Accelerometer::ConfigureSampleRate(int sample_rate) {
+    // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+    const float min_interval = FIFO_SAMPLE_DT;
+    _fifo_empty_interval_us  = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+    _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
 
-	// recompute FIFO empty interval (us) with actual sample limit
-	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+    // recompute FIFO empty interval (us) with actual sample limit
+    _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
 
-	ConfigureFIFOWatermark(_fifo_samples);
+    ConfigureFIFOWatermark(_fifo_samples);
 }
 
-void BMI055_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold
-	for (auto &r : _register_cfg) {
-		if (r.reg == Register::FIFO_CONFIG_0) {
-			r.set_bits = samples;
-			r.clear_bits = ~r.set_bits;
-		}
-	}
+void BMI055_Accelerometer::ConfigureFIFOWatermark(uint8_t samples) {
+    // FIFO watermark threshold
+    for (auto &r : _register_cfg) {
+        if (r.reg == Register::FIFO_CONFIG_0) {
+            r.set_bits   = samples;
+            r.clear_bits = ~r.set_bits;
+        }
+    }
 }
 
-bool BMI055_Accelerometer::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool BMI055_Accelerometer::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	ConfigureAccel();
+    ConfigureAccel();
 
-	return success;
+    return success;
 }
 
-int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<BMI055_Accelerometer *>(arg)->DataReady();
-	return 0;
+int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<BMI055_Accelerometer *>(arg)->DataReady();
+    return 0;
 }
 
-void BMI055_Accelerometer::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void BMI055_Accelerometer::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool BMI055_Accelerometer::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI055_Accelerometer::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+    // Setup data ready on falling edge
+    return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
-bool BMI055_Accelerometer::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI055_Accelerometer::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+    return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool BMI055_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool BMI055_Accelerometer::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t BMI055_Accelerometer::RegisterRead(Register reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
+uint8_t BMI055_Accelerometer::RegisterRead(Register reg) {
+    uint8_t cmd[2]{};
+    cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+    transfer(cmd, cmd, sizeof(cmd));
+    return cmd[1];
 }
 
-void BMI055_Accelerometer::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	transfer(cmd, cmd, sizeof(cmd));
+void BMI055_Accelerometer::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t cmd[2]{(uint8_t)reg, value};
+    transfer(cmd, cmd, sizeof(cmd));
 }
 
-void BMI055_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
+void BMI055_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
 
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+    uint8_t val = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
 
-bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) {
+    FIFOTransferBuffer buffer{};
+    const size_t       transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
 
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
+    if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return false;
+    }
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = samples;
-	accel.dt = FIFO_SAMPLE_DT;
+    sensor_accel_fifo_s accel{};
+    accel.timestamp_sample = timestamp_sample;
+    accel.samples          = samples;
+    accel.dt               = FIFO_SAMPLE_DT;
 
-	for (int i = 0; i < samples; i++) {
-		const FIFO::DATA &fifo_sample = buffer.f[i];
+    for (int i = 0; i < samples; i++) {
+        const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		// acc_x_msb<11:4> + acc_x_lsb<3:0>
-		const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
-		const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
-		const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
+        // acc_x_msb<11:4> + acc_x_lsb<3:0>
+        const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
+        const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
+        const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
 
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel_x;
-		accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-	}
+        // sensor's frame is +x forward, +y left, +z up
+        //  flip y & z to publish right handed with z down (x forward, y right, z down)
+        accel.x[i] = accel_x;
+        accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+        accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+    }
 
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+    _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
 
-	_px4_accel.updateFIFO(accel);
+    _px4_accel.updateFIFO(accel);
 
-	return true;
+    return true;
 }
 
-void BMI055_Accelerometer::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
+void BMI055_Accelerometer::FIFOReset() {
+    perf_count(_fifo_reset_perf);
 
-	// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
-	RegisterWrite(Register::FIFO_CONFIG_0, 0);
+    // FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
+    RegisterWrite(Register::FIFO_CONFIG_0, 0);
 
-	// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
-	RegisterWrite(Register::FIFO_CONFIG_1, 0);
+    // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
+    RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+    // reset while FIFO is disabled
+    _drdy_timestamp_sample.store(0);
 
-	// FIFO_CONFIG_0: restore FIFO watermark
-	// FIFO_CONFIG_1: re-enable FIFO
-	for (const auto &r : _register_cfg) {
-		if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
-			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
-		}
-	}
+    // FIFO_CONFIG_0: restore FIFO watermark
+    // FIFO_CONFIG_1: re-enable FIFO
+    for (const auto &r : _register_cfg) {
+        if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
+            RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+        }
+    }
 }
 
-void BMI055_Accelerometer::UpdateTemperature()
-{
-	// The slope of the temperature sensor is 0.5K/LSB, its center temperature is 23°C [(ACC 0x08) temp = 0x00].
-	// The register contains the current chip temperature represented in two’s complement format.
-	float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;
+void BMI055_Accelerometer::UpdateTemperature() {
+    // The slope of the temperature sensor is 0.5K/LSB, its center temperature is 23°C [(ACC 0x08) temp = 0x00].
+    // The register contains the current chip temperature represented in two’s complement format.
+    float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;
 
-	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+    if (PX4_ISFINITE(temperature)) {
+        _px4_accel.set_temperature(temperature);
 
-	} else {
-		perf_count(_bad_transfer_perf);
-	}
+    } else {
+        perf_count(_bad_transfer_perf);
+    }
 }
 
 } // namespace Bosch::BMI055::Accelerometer
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.hpp b/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.hpp
index 31906c8d96..29ac1ee1cc 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.hpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055_Accelerometer.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -39,87 +16,86 @@
 
 #include "Bosch_BMI055_Accelerometer_Registers.hpp"
 
-namespace Bosch::BMI055::Accelerometer
-{
+namespace Bosch::BMI055::Accelerometer {
 
-class BMI055_Accelerometer : public BMI055
-{
+class BMI055_Accelerometer : public BMI055 {
 public:
-	BMI055_Accelerometer(const I2CSPIDriverConfig &config);
-	~BMI055_Accelerometer() override;
+    BMI055_Accelerometer(const I2CSPIDriverConfig &config);
+    ~BMI055_Accelerometer() override;
 
-	void RunImpl() override;
-	void print_status() override;
+    void RunImpl() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr uint32_t RATE{2000}; // 2000 Hz
-	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
-
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Configure();
-	void ConfigureAccel();
-	void ConfigureSampleRate(int sample_rate = 0);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
-	void FIFOReset();
-
-	void UpdateTemperature();
-
-	PX4Accelerometer _px4_accel;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{7};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register                    | Set bits, Clear bits
-		{ Register::PMU_RANGE,         PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear},
-		{ Register::ACCD_HBW,          ACCD_HBW_BIT::data_high_bw, 0},
-		{ Register::INT_EN_1,          INT_EN_1_BIT::int_fwm_en, 0},
-		{ Register::INT_MAP_1,         INT_MAP_1_BIT::int1_fwm, 0},
-		{ Register::INT_OUT_CTRL,      0, INT_OUT_CTRL_BIT::int1_od | INT_OUT_CTRL_BIT::int1_lvl},
-		{ Register::FIFO_CONFIG_0,     0, 0 }, // fifo_water_mark_level_trigger_retain<5:0>
-		{ Register::FIFO_CONFIG_1,     FIFO_CONFIG_1_BIT::fifo_mode, 0},
-	};
+    void exit_and_cleanup() override;
+
+    // Sensor Configuration
+    static constexpr uint32_t RATE{2000}; // 2000 Hz
+    static constexpr float    FIFO_SAMPLE_DT{1e6f / RATE};
+
+    static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+
+    // Transfer data
+    struct FIFOTransferBuffer {
+        uint8_t    cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+        FIFO::DATA f[FIFO_MAX_SAMPLES]{};
+    };
+
+    // ensure no struct padding
+    static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Configure();
+    void ConfigureAccel();
+    void ConfigureSampleRate(int sample_rate = 0);
+    void ConfigureFIFOWatermark(uint8_t samples);
+
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
+
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+    void FIFOReset();
+
+    void UpdateTemperature();
+
+    PX4Accelerometer _px4_accel;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: bad transfer")};
+    perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO empty")};
+    perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO overflow")};
+    perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO reset")};
+    perf_counter_t _drdy_missed_perf{nullptr};
+
+    uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{7};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register                    | Set bits, Clear bits
+        {Register::PMU_RANGE, PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear},
+        {Register::ACCD_HBW, ACCD_HBW_BIT::data_high_bw, 0},
+        {Register::INT_EN_1, INT_EN_1_BIT::int_fwm_en, 0},
+        {Register::INT_MAP_1, INT_MAP_1_BIT::int1_fwm, 0},
+        {Register::INT_OUT_CTRL, 0, INT_OUT_CTRL_BIT::int1_od | INT_OUT_CTRL_BIT::int1_lvl},
+        {Register::FIFO_CONFIG_0, 0, 0}, // fifo_water_mark_level_trigger_retain<5:0>
+        {Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::fifo_mode, 0},
+    };
 };
 
 } // namespace Bosch::BMI055::Accelerometer
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.cpp b/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.cpp
index cf8a2495ca..c4e337cc1b 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.cpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.cpp
@@ -1,463 +1,417 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMI055_Gyroscope.hpp"
 
 using namespace time_literals;
 
-namespace Bosch::BMI055::Gyroscope
-{
+namespace Bosch::BMI055::Gyroscope {
 
 BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) :
-	BMI055(config),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
-	}
-
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-}
+    BMI055(config),
+    _px4_gyro(get_device_id(), config.rotation) {
+    if (config.drdy_gpio != 0) {
+        _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME "_gyro: DRDY missed");
+    }
 
-BMI055_Gyroscope::~BMI055_Gyroscope()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
+    ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
 }
 
-void BMI055_Gyroscope::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+BMI055_Gyroscope::~BMI055_Gyroscope() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_fifo_empty_perf);
+    perf_free(_fifo_overflow_perf);
+    perf_free(_fifo_reset_perf);
+    perf_free(_drdy_missed_perf);
 }
 
-void BMI055_Gyroscope::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
+void BMI055_Gyroscope::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-int BMI055_Gyroscope::probe()
-{
-	const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID);
+void BMI055_Gyroscope::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	if (CHIP_ID != chip_id) {
-		DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", CHIP_ID);
-		return PX4_ERROR;
-	}
+    PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
 
-	return PX4_OK;
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_fifo_empty_perf);
+    perf_print_counter(_fifo_overflow_perf);
+    perf_print_counter(_fifo_reset_perf);
+    perf_print_counter(_drdy_missed_perf);
 }
 
-void BMI055_Gyroscope::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
-		RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(25_ms);
-		break;
+int BMI055_Gyroscope::probe() {
+    const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID);
 
-	case STATE::WAIT_FOR_RESET:
-		if (RegisterRead(Register::CHIP_ID) == chip_id) {
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(1_ms);
+    if (CHIP_ID != chip_id) {
+        DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", CHIP_ID);
+        return PX4_ERROR;
+    }
 
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start reading from FIFO
-			_state = STATE::FIFO_READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = 0;
-
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			// always check current FIFO status/count
-			bool success = false;
-			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
-
-			if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
-				FIFOReset();
-				perf_count(_fifo_overflow_perf);
-
-			} else {
-				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
-
-				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
-					// not technically an overflow, but more samples than we expected or can publish
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_frame_counter == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else if (fifo_frame_counter >= 1) {
-
-					uint8_t samples = fifo_frame_counter;
-
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
-					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
-						success = true;
-
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-			}
-		}
+    return PX4_OK;
+}
 
-		break;
-	}
+void BMI055_Gyroscope::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
+        RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        ScheduleDelayed(25_ms);
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+        if (RegisterRead(Register::CHIP_ID) == chip_id) {
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(1_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start reading from FIFO
+            _state = STATE::FIFO_READ;
+
+            if (DataReadyInterruptConfigure()) {
+                _data_ready_interrupt_enabled = true;
+
+                // backup schedule as a watchdog timeout
+                ScheduleDelayed(100_ms);
+
+            } else {
+                _data_ready_interrupt_enabled = false;
+                ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+            }
+
+            FIFOReset();
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::FIFO_READ: {
+        hrt_abstime timestamp_sample = 0;
+
+        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) < _fifo_empty_interval_us) {
+                timestamp_sample = drdy_timestamp_sample;
+
+            } else {
+                perf_count(_drdy_missed_perf);
+            }
+
+            // push backup schedule back
+            ScheduleDelayed(_fifo_empty_interval_us * 2);
+        }
+
+        // always check current FIFO status/count
+        bool          success     = false;
+        const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+
+        if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
+            FIFOReset();
+            perf_count(_fifo_overflow_perf);
+
+        } else {
+            const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
+
+            if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+                // not technically an overflow, but more samples than we expected or can publish
+                FIFOReset();
+                perf_count(_fifo_overflow_perf);
+
+            } else if (fifo_frame_counter == 0) {
+                perf_count(_fifo_empty_perf);
+
+            } else if (fifo_frame_counter >= 1) {
+                uint8_t samples = fifo_frame_counter;
+
+                // tolerate minor jitter, leave sample to next iteration if behind by only 1
+                if (samples == _fifo_samples + 1) {
+                    // sample timestamp set from data ready already corresponds to _fifo_samples
+                    if (timestamp_sample == 0) {
+                        timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+                    }
+
+                    samples--;
+                }
+
+                if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
+                    success = true;
+
+                    if (_failure_count > 0) {
+                        _failure_count--;
+                    }
+                }
+            }
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+        }
+    }
+
+    break;
+    }
 }
 
-void BMI055_Gyroscope::ConfigureGyro()
-{
-	const uint8_t RANGE = RegisterRead(Register::RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
-
-	switch (RANGE) {
-	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
-		break;
-
-	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
-		break;
-
-	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
-		break;
-
-	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
-		break;
-
-	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
-		break;
-	}
+void BMI055_Gyroscope::ConfigureGyro() {
+    const uint8_t RANGE = RegisterRead(Register::RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
+
+    switch (RANGE) {
+    case gyro_range_2000_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 16.384f));
+        _px4_gyro.set_range(math::radians(2000.f));
+        break;
+
+    case gyro_range_1000_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 32.768f));
+        _px4_gyro.set_range(math::radians(1000.f));
+        break;
+
+    case gyro_range_500_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 65.536f));
+        _px4_gyro.set_range(math::radians(500.f));
+        break;
+
+    case gyro_range_250_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 131.072f));
+        _px4_gyro.set_range(math::radians(250.f));
+        break;
+
+    case gyro_range_125_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 262.144f));
+        _px4_gyro.set_range(math::radians(125.f));
+        break;
+    }
 }
 
-void BMI055_Gyroscope::ConfigureSampleRate(int sample_rate)
-{
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+void BMI055_Gyroscope::ConfigureSampleRate(int sample_rate) {
+    // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+    const float min_interval = FIFO_SAMPLE_DT;
+    _fifo_empty_interval_us  = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+    _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
 
-	// recompute FIFO empty interval (us) with actual sample limit
-	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+    // recompute FIFO empty interval (us) with actual sample limit
+    _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
 
-	ConfigureFIFOWatermark(_fifo_samples);
+    ConfigureFIFOWatermark(_fifo_samples);
 }
 
-void BMI055_Gyroscope::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold
-	for (auto &r : _register_cfg) {
-		if (r.reg == Register::FIFO_CONFIG_0) {
-			r.set_bits = samples;
-			r.clear_bits = ~r.set_bits | FIFO_CONFIG_0_BIT::tag;
-		}
-	}
+void BMI055_Gyroscope::ConfigureFIFOWatermark(uint8_t samples) {
+    // FIFO watermark threshold
+    for (auto &r : _register_cfg) {
+        if (r.reg == Register::FIFO_CONFIG_0) {
+            r.set_bits   = samples;
+            r.clear_bits = ~r.set_bits | FIFO_CONFIG_0_BIT::tag;
+        }
+    }
 }
 
-bool BMI055_Gyroscope::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool BMI055_Gyroscope::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	ConfigureGyro();
+    ConfigureGyro();
 
-	return success;
+    return success;
 }
 
-int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<BMI055_Gyroscope *>(arg)->DataReady();
-	return 0;
+int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<BMI055_Gyroscope *>(arg)->DataReady();
+    return 0;
 }
 
-void BMI055_Gyroscope::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void BMI055_Gyroscope::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool BMI055_Gyroscope::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI055_Gyroscope::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+    // Setup data ready on falling edge
+    return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
-bool BMI055_Gyroscope::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI055_Gyroscope::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+    return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool BMI055_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool BMI055_Gyroscope::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t BMI055_Gyroscope::RegisterRead(Register reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
+uint8_t BMI055_Gyroscope::RegisterRead(Register reg) {
+    uint8_t cmd[2]{};
+    cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+    transfer(cmd, cmd, sizeof(cmd));
+    return cmd[1];
 }
 
-void BMI055_Gyroscope::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	transfer(cmd, cmd, sizeof(cmd));
+void BMI055_Gyroscope::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t cmd[2]{(uint8_t)reg, value};
+    transfer(cmd, cmd, sizeof(cmd));
 }
 
-void BMI055_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
+void BMI055_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
 
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+    uint8_t val = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
 
-bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) {
+    FIFOTransferBuffer buffer{};
+    const size_t       transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
 
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
+    if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return false;
+    }
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+    sensor_gyro_fifo_s gyro{};
+    gyro.timestamp_sample = timestamp_sample;
+    gyro.samples          = samples;
+    gyro.dt               = FIFO_SAMPLE_DT;
 
-	for (int i = 0; i < samples; i++) {
-		const FIFO::DATA &fifo_sample = buffer.f[i];
+    for (int i = 0; i < samples; i++) {
+        const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+        const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+        const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+        const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-	}
+        // sensor's frame is +x forward, +y left, +z up
+        //  flip y & z to publish right handed with z down (x forward, y right, z down)
+        gyro.x[i] = gyro_x;
+        gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+        gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+    }
 
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+    _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
 
-	_px4_gyro.updateFIFO(gyro);
+    _px4_gyro.updateFIFO(gyro);
 
-	return true;
+    return true;
 }
 
-void BMI055_Gyroscope::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
+void BMI055_Gyroscope::FIFOReset() {
+    perf_count(_fifo_reset_perf);
 
-	// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
-	RegisterWrite(Register::FIFO_CONFIG_0, 0);
+    // FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
+    RegisterWrite(Register::FIFO_CONFIG_0, 0);
 
-	// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
-	RegisterWrite(Register::FIFO_CONFIG_1, 0);
+    // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
+    RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+    // reset while FIFO is disabled
+    _drdy_timestamp_sample.store(0);
 
-	// FIFO_CONFIG_0: restore FIFO watermark
-	// FIFO_CONFIG_1: re-enable FIFO
-	for (const auto &r : _register_cfg) {
-		if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
-			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
-		}
-	}
+    // FIFO_CONFIG_0: restore FIFO watermark
+    // FIFO_CONFIG_1: re-enable FIFO
+    for (const auto &r : _register_cfg) {
+        if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
+            RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+        }
+    }
 }
 
 } // namespace Bosch::BMI055::Gyroscope
diff --git a/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.hpp b/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.hpp
index 13c0468f0d..9371eaa455 100644
--- a/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.hpp
+++ b/apps/peripheral/sensor/imu/bmi055/BMI055_Gyroscope.hpp
@@ -1,35 +1,16 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#ifndef __BMI055_GYROSCOPE_H__
+#define __BMI055_GYROSCOPE_H__
+
 
 #pragma once
 
@@ -39,86 +20,87 @@
 
 #include "Bosch_BMI055_Gyroscope_Registers.hpp"
 
-namespace Bosch::BMI055::Gyroscope
-{
+namespace Bosch::BMI055::Gyroscope {
 
-class BMI055_Gyroscope : public BMI055
-{
+class BMI055_Gyroscope : public BMI055 {
 public:
-	BMI055_Gyroscope(const I2CSPIDriverConfig &config);
-	~BMI055_Gyroscope() override;
+    BMI055_Gyroscope(const I2CSPIDriverConfig &config);
+    ~BMI055_Gyroscope() override;
 
-	void RunImpl() override;
-	void print_status() override;
+    void RunImpl() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr uint32_t RATE{2000}; // 2000 Hz
-	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
-
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Configure();
-	void ConfigureGyro();
-	void ConfigureSampleRate(int sample_rate = 0);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
-	void FIFOReset();
-
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{8};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register                    | Set bits, Clear bits
-		{ Register::RANGE,             RANGE_BIT::gyro_range_2000_dps, 0 },
-		{ Register::RATE_HBW,          RATE_HBW_BIT::data_high_bw, 0 },
-		{ Register::INT_EN_0,          INT_EN_0_BIT::fifo_en, 0 },
-		{ Register::INT_EN_1,          0, INT_EN_1_BIT::int1_od | INT_EN_1_BIT::int1_lvl },
-		{ Register::INT_MAP_1,         INT_MAP_1_BIT::int1_fifo, 0 },
-		{ Register::FIFO_WM_ENABLE,    FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
-		{ Register::FIFO_CONFIG_0,     0, FIFO_CONFIG_0_BIT::tag }, // fifo_water_mark_level_trigger_retain<6:0>
-		{ Register::FIFO_CONFIG_1,     FIFO_CONFIG_1_BIT::fifo_mode, 0 },
-	};
+    void exit_and_cleanup() override;
+
+    // Sensor Configuration
+    static constexpr uint32_t RATE{2000}; // 2000 Hz
+    static constexpr float    FIFO_SAMPLE_DT{1e6f / RATE};
+
+    static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+
+    // Transfer data
+    struct FIFOTransferBuffer {
+        uint8_t    cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+        FIFO::DATA f[FIFO_MAX_SAMPLES]{};
+    };
+
+    // ensure no struct padding
+    static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Configure();
+    void ConfigureGyro();
+    void ConfigureSampleRate(int sample_rate = 0);
+    void ConfigureFIFOWatermark(uint8_t samples);
+
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
+
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+    void FIFOReset();
+
+    PX4Gyroscope _px4_gyro;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: bad transfer")};
+    perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO empty")};
+    perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO overflow")};
+    perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO reset")};
+    perf_counter_t _drdy_missed_perf{nullptr};
+
+    uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{8};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register                    | Set bits, Clear bits
+        {Register::RANGE, RANGE_BIT::gyro_range_2000_dps, 0},
+        {Register::RATE_HBW, RATE_HBW_BIT::data_high_bw, 0},
+        {Register::INT_EN_0, INT_EN_0_BIT::fifo_en, 0},
+        {Register::INT_EN_1, 0, INT_EN_1_BIT::int1_od | INT_EN_1_BIT::int1_lvl},
+        {Register::INT_MAP_1, INT_MAP_1_BIT::int1_fifo, 0},
+        {Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0},
+        {Register::FIFO_CONFIG_0, 0, FIFO_CONFIG_0_BIT::tag}, // fifo_water_mark_level_trigger_retain<6:0>
+        {Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::fifo_mode, 0},
+    };
 };
 
 } // namespace Bosch::BMI055::Gyroscope
+
+#endif // __BMI055_GYROSCOPE_H__
diff --git a/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp b/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
index f95669f293..531130055b 100644
--- a/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
+++ b/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
@@ -1,40 +1,20 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#ifndef __BOSCH_BMI055_ACCELEROMETER_REGISTERS_H__
+#define __BOSCH_BMI055_ACCELEROMETER_REGISTERS_H__
+
 
 #pragma once
 
-namespace Bosch::BMI055::Accelerometer
-{
+namespace Bosch::BMI055::Accelerometer {
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -47,111 +27,113 @@ static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
-static constexpr uint8_t DIR_READ = 0x80;
+static constexpr uint8_t  DIR_READ  = 0x80;
 
 static constexpr uint8_t chip_id = 0b11111010;
 
 enum class Register : uint8_t {
-	BGW_CHIPID    = 0x00,
+    BGW_CHIPID = 0x00,
 
-	ACCD_TEMP     = 0x08,
+    ACCD_TEMP = 0x08,
 
-	INT_STATUS_1  = 0x0A,
+    INT_STATUS_1 = 0x0A,
 
-	FIFO_STATUS   = 0x0E,
-	PMU_RANGE     = 0x0F,
+    FIFO_STATUS = 0x0E,
+    PMU_RANGE   = 0x0F,
 
-	ACCD_HBW      = 0x13,
-	BGW_SOFTRESET = 0x14,
+    ACCD_HBW      = 0x13,
+    BGW_SOFTRESET = 0x14,
 
-	INT_EN_1      = 0x17,
+    INT_EN_1 = 0x17,
 
-	INT_MAP_1     = 0x1A,
+    INT_MAP_1 = 0x1A,
 
-	INT_OUT_CTRL  = 0x20,
+    INT_OUT_CTRL = 0x20,
 
-	FIFO_CONFIG_0 = 0x30,
+    FIFO_CONFIG_0 = 0x30,
 
-	FIFO_CONFIG_1 = 0x3E,
-	FIFO_DATA     = 0x3F,
+    FIFO_CONFIG_1 = 0x3E,
+    FIFO_DATA     = 0x3F,
 };
 
 // INT_STATUS_1
 enum INT_STATUS_1_BIT : uint8_t {
-	data_int      = Bit7,
-	fifo_wm_int   = Bit6,
-	fifo_full_int = Bit5,
+    data_int      = Bit7,
+    fifo_wm_int   = Bit6,
+    fifo_full_int = Bit5,
 };
 
 // FIFO_STATUS
 enum FIFO_STATUS_BIT : uint8_t {
-	fifo_overrun       = Bit7,
-	fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // fifo_frame_counter<6:0>
+    fifo_overrun       = Bit7,
+    fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // fifo_frame_counter<6:0>
 };
 
 // ACCD_HBW
 enum ACCD_HBW_BIT : uint8_t {
-	data_high_bw = Bit7, // 1 -> unfiltered
+    data_high_bw = Bit7, // 1 -> unfiltered
 };
 
 // PMU_RANGE
 enum PMU_RANGE_BIT : uint8_t {
-	// range<3:0>
-	range_16g_set   = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
-	range_16g_clear = Bit1 | Bit0,
+    // range<3:0>
+    range_16g_set   = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
+    range_16g_clear = Bit1 | Bit0,
 
-	range_8g_set    = Bit3,        //  ́1000b ́ -> ±8g range
-	range_8g_clear  = Bit2 | Bit1 | Bit0,
+    range_8g_set   = Bit3, //  ́1000b ́ -> ±8g range
+    range_8g_clear = Bit2 | Bit1 | Bit0,
 
-	range_4g_set    = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
-	range_4g_clear  = Bit3 | Bit1,
+    range_4g_set   = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
+    range_4g_clear = Bit3 | Bit1,
 
-	range_2g_set    = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
-	range_2g_clear  = Bit3 | Bit2,
+    range_2g_set   = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
+    range_2g_clear = Bit3 | Bit2,
 };
 
 // INT_EN_1
 enum INT_EN_1_BIT : uint8_t {
-	int_fwm_en   = Bit6,
-	int_ffull_en = Bit5,
-	data_en      = Bit4,
+    int_fwm_en   = Bit6,
+    int_ffull_en = Bit5,
+    data_en      = Bit4,
 };
 
 // INT_MAP_1
 enum INT_MAP_1_BIT : uint8_t {
-	int2_data  = Bit7,
-	int2_fwm   = Bit6,
-	int2_ffull = Bit5,
+    int2_data  = Bit7,
+    int2_fwm   = Bit6,
+    int2_ffull = Bit5,
 
-	int1_ffull = Bit2,
-	int1_fwm   = Bit1,
-	int1_data  = Bit0,
+    int1_ffull = Bit2,
+    int1_fwm   = Bit1,
+    int1_data  = Bit0,
 };
 
 // INT_OUT_CTRL
 enum INT_OUT_CTRL_BIT : uint8_t {
-	int1_od  = Bit1,
-	int1_lvl = Bit0,
+    int1_od  = Bit1,
+    int1_lvl = Bit0,
 };
 
 // FIFO_CONFIG_1
 enum FIFO_CONFIG_1_BIT : uint8_t {
-	fifo_mode = Bit6,
+    fifo_mode = Bit6,
 };
 
-namespace FIFO
-{
+namespace FIFO {
 struct DATA {
-	uint8_t ACCD_X_LSB;
-	uint8_t ACCD_X_MSB;
-	uint8_t ACCD_Y_LSB;
-	uint8_t ACCD_Y_MSB;
-	uint8_t ACCD_Z_LSB;
-	uint8_t ACCD_Z_MSB;
+    uint8_t ACCD_X_LSB;
+    uint8_t ACCD_X_MSB;
+    uint8_t ACCD_Y_LSB;
+    uint8_t ACCD_Y_MSB;
+    uint8_t ACCD_Z_LSB;
+    uint8_t ACCD_Z_MSB;
 };
+
 static_assert(sizeof(DATA) == 6);
 
 static constexpr size_t SIZE = sizeof(DATA) * 32; // up to 32 frames of accelerometer data
 
 } // namespace FIFO
 } // namespace Bosch::BMI055::Accelerometer
+
+#endif // __BOSCH_BMI055_ACCELEROMETER_REGISTERS_H__
diff --git a/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Gyroscope_Registers.hpp b/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Gyroscope_Registers.hpp
index 5d6ce87846..c2fd505518 100644
--- a/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Gyroscope_Registers.hpp
+++ b/apps/peripheral/sensor/imu/bmi055/Bosch_BMI055_Gyroscope_Registers.hpp
@@ -1,40 +1,20 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#ifndef __BOSCH_BMI055_GYROSCOPE_REGISTERS_H__
+#define __BOSCH_BMI055_GYROSCOPE_REGISTERS_H__
+
 
 #pragma once
 
-namespace Bosch::BMI055::Gyroscope
-{
+namespace Bosch::BMI055::Gyroscope {
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -47,93 +27,93 @@ static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
-static constexpr uint8_t DIR_READ = 0x80;
+static constexpr uint8_t  DIR_READ  = 0x80;
 
 static constexpr uint8_t chip_id = 0x0F;
 
 enum class Register : uint8_t {
-	CHIP_ID        = 0x00,
+    CHIP_ID = 0x00,
 
-	FIFO_STATUS    = 0x0E,
-	RANGE          = 0x0F,
+    FIFO_STATUS = 0x0E,
+    RANGE       = 0x0F,
 
-	RATE_HBW       = 0x13,
-	BGW_SOFTRESET  = 0x14,
-	INT_EN_0       = 0x15,
-	INT_EN_1       = 0x16,
+    RATE_HBW      = 0x13,
+    BGW_SOFTRESET = 0x14,
+    INT_EN_0      = 0x15,
+    INT_EN_1      = 0x16,
 
-	INT_MAP_1      = 0x18,
+    INT_MAP_1 = 0x18,
 
-	FIFO_WM_ENABLE = 0x1E,
+    FIFO_WM_ENABLE = 0x1E,
 
-	FIFO_CONFIG_0  = 0x3D,
-	FIFO_CONFIG_1  = 0x3E,
-	FIFO_DATA      = 0x3F,
+    FIFO_CONFIG_0 = 0x3D,
+    FIFO_CONFIG_1 = 0x3E,
+    FIFO_DATA     = 0x3F,
 };
 
 // FIFO_STATUS
 enum FIFO_STATUS_BIT : uint8_t {
-	fifo_overrun       = Bit7,
-	fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // fifo_frame_counter<6:0>
+    fifo_overrun       = Bit7,
+    fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // fifo_frame_counter<6:0>
 };
 
 // RANGE
 enum RANGE_BIT : uint8_t {
-	gyro_range_2000_dps = 0x00, // ±2000
-	gyro_range_1000_dps = 0x01, // ±1000
-	gyro_range_500_dps  = 0x02, // ±500
-	gyro_range_250_dps  = 0x04, // ±250
-	gyro_range_125_dps  = 0x05, // ±125
+    gyro_range_2000_dps = 0x00, // ±2000
+    gyro_range_1000_dps = 0x01, // ±1000
+    gyro_range_500_dps  = 0x02, // ±500
+    gyro_range_250_dps  = 0x04, // ±250
+    gyro_range_125_dps  = 0x05, // ±125
 };
 
 // RATE_HBW
 enum RATE_HBW_BIT : uint8_t {
-	data_high_bw = Bit7, //  1 -> unfiltered
+    data_high_bw = Bit7, //  1 -> unfiltered
 };
 
 // INT_EN_0
 enum INT_EN_0_BIT : uint8_t {
-	data_en = Bit7,
-	fifo_en = Bit6,
+    data_en = Bit7,
+    fifo_en = Bit6,
 };
 
 // INT_EN_1
 enum INT_EN_1_BIT : uint8_t {
-	int1_od  = Bit1,
-	int1_lvl = Bit0,
+    int1_od  = Bit1,
+    int1_lvl = Bit0,
 };
 
 // INT_MAP_1
 enum INT_MAP_1_BIT : uint8_t {
-	int1_fifo = Bit2,
-	int1_data = Bit0,
+    int1_fifo = Bit2,
+    int1_data = Bit0,
 };
 
 // FIFO_WM_ENABLE
 enum FIFO_WM_ENABLE_BIT : uint8_t {
-	fifo_wm_enable  = Bit7,
+    fifo_wm_enable = Bit7,
 };
 
 // FIFO_CONFIG_0
 enum FIFO_CONFIG_0_BIT : uint8_t {
-	tag = Bit7,
+    tag = Bit7,
 };
 
 // FIFO_CONFIG_1
 enum FIFO_CONFIG_1_BIT : uint8_t {
-	fifo_mode = Bit6,
+    fifo_mode = Bit6,
 };
 
-namespace FIFO
-{
+namespace FIFO {
 struct DATA {
-	uint8_t RATE_X_LSB;
-	uint8_t RATE_X_MSB;
-	uint8_t RATE_Y_LSB;
-	uint8_t RATE_Y_MSB;
-	uint8_t RATE_Z_LSB;
-	uint8_t RATE_Z_MSB;
+    uint8_t RATE_X_LSB;
+    uint8_t RATE_X_MSB;
+    uint8_t RATE_Y_LSB;
+    uint8_t RATE_Y_MSB;
+    uint8_t RATE_Z_LSB;
+    uint8_t RATE_Z_MSB;
 };
+
 static_assert(sizeof(DATA) == 6);
 
 // 100 frames of data in FIFO mode
@@ -141,3 +121,5 @@ static constexpr size_t SIZE = sizeof(DATA) * 100;
 
 } // namespace FIFO
 } // namespace Bosch::BMI055::Gyroscope
+
+#endif // __BOSCH_BMI055_GYROSCOPE_REGISTERS_H__
diff --git a/apps/peripheral/sensor/imu/bmi055/CMakeLists.txt b/apps/peripheral/sensor/imu/bmi055/CMakeLists.txt
deleted file mode 100644
index bf24faffde..0000000000
--- a/apps/peripheral/sensor/imu/bmi055/CMakeLists.txt
+++ /dev/null
@@ -1,54 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2020 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__imu__bosch__bmi055
-	MAIN bmi055
-	COMPILE_FLAGS
-	SRCS
-		Bosch_BMI055_Accelerometer_Registers.hpp
-		Bosch_BMI055_Gyroscope_Registers.hpp
-
-		BMI055.cpp
-		BMI055.hpp
-		BMI055_Accelerometer.cpp
-		BMI055_Accelerometer.hpp
-		BMI055_Gyroscope.cpp
-		BMI055_Gyroscope.hpp
-
-		bmi055_main.cpp
-	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
-		px4_work_queue
-	)
diff --git a/apps/peripheral/sensor/imu/bmi055/Kconfig b/apps/peripheral/sensor/imu/bmi055/Kconfig
index 473a97021b..cf8fd59f3b 100644
--- a/apps/peripheral/sensor/imu/bmi055/Kconfig
+++ b/apps/peripheral/sensor/imu/bmi055/Kconfig
@@ -1,5 +1,7 @@
-menuconfig DRIVERS_IMU_BOSCH_BMI055
+menuconfig DRV_USING_BOSCH_BMI055
 	bool "bosch bmi055"
+	select PKG_USING_GYROSCOPE
+	select PKG_USING_ACCELEROMETER
 	default n
 	---help---
 		Enable support for bosch bmi055
diff --git a/apps/peripheral/sensor/imu/bmi055/SConscript b/apps/peripheral/sensor/imu/bmi055/SConscript
new file mode 100644
index 0000000000..20b5b46b5a
--- /dev/null
+++ b/apps/peripheral/sensor/imu/bmi055/SConscript
@@ -0,0 +1,13 @@
+import os
+import sys
+import subprocess
+from building import *
+
+cwd = GetCurrentDir()
+
+inc = []
+src = Glob("*.cpp")
+
+objs = DefineGroup("drv/bmi055", src, depend=["DRV_USING_PWM_OUT"])
+
+Return("objs")
diff --git a/apps/peripheral/sensor/imu/bmi055/bmi055_main.cpp b/apps/peripheral/sensor/imu/bmi055/bmi055_main.cpp
index 8ce9fd959c..37d32a0c75 100644
--- a/apps/peripheral/sensor/imu/bmi055/bmi055_main.cpp
+++ b/apps/peripheral/sensor/imu/bmi055/bmi055_main.cpp
@@ -1,101 +1,78 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
 
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
+#include <getopt/getopt.h>
+#include <module/module_params.h>
+#include <module/module_command.h>
 
 #include "BMI055.hpp"
 
-void BMI055::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("bmi055", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
-	PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void BMI055::print_usage() {
+    PRINT_MODULE_USAGE_NAME("bmi055", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
+    PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int bmi055_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = BMI055;
-	BusCLIArguments cli{false, true};
-	uint16_t type = 0;
-	cli.default_spi_frequency = 10000000;
-	const char *name = MODULE_NAME;
+extern "C" int bmi055_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = BMI055;
+    BusCLIArguments cli{false, true};
+    uint16_t        type      = 0;
+    cli.default_spi_frequency = 10000000;
+    const char *name          = MODULE_NAME;
 
-	while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
-		switch (ch) {
-		case 'A':
-			type = DRV_ACC_DEVTYPE_BMI055;
-			name = MODULE_NAME "_accel";
-			break;
+    while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
+        switch (ch) {
+        case 'A':
+            type = DRV_ACC_DEVTYPE_BMI055;
+            name = MODULE_NAME "_accel";
+            break;
 
-		case 'G':
-			type = DRV_GYR_DEVTYPE_BMI055;
-			name = MODULE_NAME "_gyro";
-			break;
+        case 'G':
+            type = DRV_GYR_DEVTYPE_BMI055;
+            name = MODULE_NAME "_gyro";
+            break;
 
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb || type == 0) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb || type == 0) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(name, cli, type);
+    BusInstanceIterator iterator(name, cli, type);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088.cpp b/apps/peripheral/sensor/imu/bmi088/BMI088.cpp
index 0c45fd3a2d..e22ee2034c 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088.cpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088.cpp
@@ -1,88 +1,61 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMI088.hpp"
 
 #include "BMI088_Accelerometer.hpp"
 #include "BMI088_Gyroscope.hpp"
 
-I2CSPIDriverBase *BMI088::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	BMI088 *instance = nullptr;
+I2CSPIDriverBase *BMI088::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    BMI088 *instance = nullptr;
 
-	if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI088) {
-		instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(config);
+    if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI088) {
+        instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(config);
 
-	} else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI088) {
-		instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(config);
-	}
+    } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI088) {
+        instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(config);
+    }
 
-	if (!instance) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
+    if (!instance) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
 
-	if (OK != instance->init()) {
-		delete instance;
-		return nullptr;
-	}
+    if (OK != instance->init()) {
+        delete instance;
+        return nullptr;
+    }
 
-	return instance;
+    return instance;
 }
 
 BMI088::BMI088(const I2CSPIDriverConfig &config) :
-	SPI(config),
-	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio)
-{
+    SPI(config),
+    I2CSPIDriver(config),
+    _drdy_gpio(config.drdy_gpio) {
 }
 
-int BMI088::init()
-{
-	int ret = SPI::init();
+int BMI088::init() {
+    int ret = SPI::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("SPI::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool BMI088::Reset()
-{
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool BMI088::Reset() {
+    _state = STATE::RESET;
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088.hpp b/apps/peripheral/sensor/imu/bmi088/BMI088.hpp
index fa6cc22370..252f6d5d6b 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088.hpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -38,45 +15,44 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
+}
 
-class BMI088 : public device::SPI, public I2CSPIDriver<BMI088>
-{
+class BMI088 : public device::SPI, public I2CSPIDriver<BMI088> {
 public:
-	BMI088(const I2CSPIDriverConfig &config);
+    BMI088(const I2CSPIDriverConfig &config);
 
-	virtual ~BMI088() = default;
+    virtual ~BMI088() = default;
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	virtual void RunImpl() = 0;
+    virtual void RunImpl() = 0;
 
-	int init() override;
-	virtual void print_status() = 0;
+    int          init() override;
+    virtual void print_status() = 0;
 
 protected:
+    bool Reset();
 
-	bool Reset();
-
-	const spi_drdy_gpio_t _drdy_gpio;
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
+    const spi_drdy_gpio_t _drdy_gpio;
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _data_ready_interrupt_enabled{false};
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_config_check_timestamp{0};
+    hrt_abstime _temperature_update_timestamp{0};
+    int         _failure_count{0};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_RESET,
-		FIFO_READ,
-	} _state{STATE::RESET};
+    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                     _data_ready_interrupt_enabled{false};
 
-	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        FIFO_RESET,
+        FIFO_READ,
+    } _state{STATE::RESET};
 
+    uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 };
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.cpp b/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.cpp
index 8b0f3c65f1..99c1ad5521 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.cpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMI088_Accelerometer.hpp"
 
@@ -37,53 +14,47 @@
 
 using namespace time_literals;
 
-namespace Bosch::BMI088::Accelerometer
-{
+namespace Bosch::BMI088::Accelerometer {
 
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
-	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
-	}
-
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+    BMI088(config),
+    _px4_accel(get_device_id(), config.rotation) {
+    if (config.drdy_gpio != 0) {
+        _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME "_accel: DRDY missed");
+    }
+
+    ConfigureSampleRate(_px4_accel.get_max_rate_hz());
 }
 
-BMI088_Accelerometer::~BMI088_Accelerometer()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
+BMI088_Accelerometer::~BMI088_Accelerometer() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_fifo_empty_perf);
+    perf_free(_fifo_overflow_perf);
+    perf_free(_fifo_reset_perf);
+    perf_free(_drdy_missed_perf);
 }
 
-void BMI088_Accelerometer::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+void BMI088_Accelerometer::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void BMI088_Accelerometer::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void BMI088_Accelerometer::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+    PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
 
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_fifo_empty_perf);
+    perf_print_counter(_fifo_overflow_perf);
+    perf_print_counter(_fifo_reset_perf);
+    perf_print_counter(_drdy_missed_perf);
 }
 
-int BMI088_Accelerometer::probe()
-{
-	/* 6.1 Serial Peripheral Interface (SPI)
+int BMI088_Accelerometer::probe() {
+    /* 6.1 Serial Peripheral Interface (SPI)
 	 * ... the accelerometer part starts always in I2C mode
 	 * (regardless of the level of the PS pin) and needs to be changed to SPI
 	 *  mode actively by sending a rising edge on the CSB1 pin
@@ -94,525 +65,506 @@ int BMI088_Accelerometer::probe()
 	 *  could perfom a dummy SPI read operation, e.g. of register ACC_CHIP_ID
 	 *  (the obtained value will be invalid).In case of read operations,
 	 */
-	RegisterRead(Register::ACC_CHIP_ID);
+    RegisterRead(Register::ACC_CHIP_ID);
 
-	const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
+    const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
 
-	if (ACC_CHIP_ID == ID_088) {
-		DEVICE_DEBUG("BMI088 Accel");
+    if (ACC_CHIP_ID == ID_088) {
+        DEVICE_DEBUG("BMI088 Accel");
 
-	} else if (ACC_CHIP_ID == ID_090L) {
-		DEVICE_DEBUG("BMI090L Accel");
+    } else if (ACC_CHIP_ID == ID_090L) {
+        DEVICE_DEBUG("BMI090L Accel");
 
-	} else {
-		DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
-		return PX4_ERROR;
-	}
+    } else {
+        DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
+        return PX4_ERROR;
+    }
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-void BMI088_Accelerometer::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
-		RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
-		DataReadyInterruptDisable();
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value.
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if ((RegisterRead(Register::ACC_CHIP_ID) == ID_088) || (RegisterRead(Register::ACC_CHIP_ID) == ID_090L)) {
-			// ACC_PWR_CONF: Power on sensor
-			RegisterWrite(Register::ACC_PWR_CONF, 0);
-
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_RESET:
-		// if configure succeeded then start reading from FIFO
-		_state = STATE::FIFO_READ;
-
-		FIFOReset();
-
-		if (DataReadyInterruptConfigure()) {
-			_data_ready_interrupt_enabled = true;
-
-			// backup schedule as a watchdog timeout
-			ScheduleDelayed(100_ms);
-
-		} else {
-			_data_ready_interrupt_enabled = false;
-			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
-
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-					samples = _fifo_samples;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_byte_counter = FIFOReadCount();
-
-				if (fifo_byte_counter >= FIFO::SIZE) {
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if ((fifo_byte_counter == 0) || (fifo_byte_counter == 0x8000)) {
-					// An empty FIFO corresponds to 0x8000
-					perf_count(_fifo_empty_perf);
-
-				} else {
-					samples = fifo_byte_counter / sizeof(FIFO::DATA);
-
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
-
-					if (samples > FIFO_MAX_SAMPLES) {
-						// not technically an overflow, but more samples than we expected or can publish
-						FIFOReset();
-						perf_count(_fifo_overflow_perf);
-						samples = 0;
-					}
-				}
-			}
-
-			bool success = false;
-
-			if (samples >= 1) {
-				if (FIFORead(timestamp_sample, samples)) {
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-
-			} else {
-				// periodically update temperature (~1 Hz)
-				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
-					UpdateTemperature();
-					_temperature_update_timestamp = now;
-				}
-			}
-		}
-
-		break;
-	}
+void BMI088_Accelerometer::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
+        RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
+        DataReadyInterruptDisable();
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value.
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+        if ((RegisterRead(Register::ACC_CHIP_ID) == ID_088) || (RegisterRead(Register::ACC_CHIP_ID) == ID_090L)) {
+            // ACC_PWR_CONF: Power on sensor
+            RegisterWrite(Register::ACC_PWR_CONF, 0);
+
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then reset the FIFO
+            _state = STATE::FIFO_RESET;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::FIFO_RESET:
+        // if configure succeeded then start reading from FIFO
+        _state = STATE::FIFO_READ;
+
+        FIFOReset();
+
+        if (DataReadyInterruptConfigure()) {
+            _data_ready_interrupt_enabled = true;
+
+            // backup schedule as a watchdog timeout
+            ScheduleDelayed(100_ms);
+
+        } else {
+            _data_ready_interrupt_enabled = false;
+            ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+        }
+
+        break;
+
+    case STATE::FIFO_READ: {
+        hrt_abstime timestamp_sample = now;
+        uint8_t     samples          = 0;
+
+        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) < _fifo_empty_interval_us) {
+                timestamp_sample = drdy_timestamp_sample;
+                samples          = _fifo_samples;
+
+            } else {
+                perf_count(_drdy_missed_perf);
+            }
+
+            // push backup schedule back
+            ScheduleDelayed(_fifo_empty_interval_us * 2);
+        }
+
+        if (samples == 0) {
+            // check current FIFO count
+            const uint16_t fifo_byte_counter = FIFOReadCount();
+
+            if (fifo_byte_counter >= FIFO::SIZE) {
+                FIFOReset();
+                perf_count(_fifo_overflow_perf);
+
+            } else if ((fifo_byte_counter == 0) || (fifo_byte_counter == 0x8000)) {
+                // An empty FIFO corresponds to 0x8000
+                perf_count(_fifo_empty_perf);
+
+            } else {
+                samples = fifo_byte_counter / sizeof(FIFO::DATA);
+
+                // tolerate minor jitter, leave sample to next iteration if behind by only 1
+                if (samples == _fifo_samples + 1) {
+                    timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+                    samples--;
+                }
+
+                if (samples > FIFO_MAX_SAMPLES) {
+                    // not technically an overflow, but more samples than we expected or can publish
+                    FIFOReset();
+                    perf_count(_fifo_overflow_perf);
+                    samples = 0;
+                }
+            }
+        }
+
+        bool success = false;
+
+        if (samples >= 1) {
+            if (FIFORead(timestamp_sample, samples)) {
+                success = true;
+
+                if (_failure_count > 0) {
+                    _failure_count--;
+                }
+            }
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+
+        } else {
+            // periodically update temperature (~1 Hz)
+            if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+                UpdateTemperature();
+                _temperature_update_timestamp = now;
+            }
+        }
+    }
+
+    break;
+    }
 }
 
-void BMI088_Accelerometer::ConfigureAccel()
-{
-	const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
-
-	switch (ACC_RANGE) {
-	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
-		break;
-
-	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
-		break;
-
-	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
-		break;
-
-	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
-		break;
-	}
+void BMI088_Accelerometer::ConfigureAccel() {
+    const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
+
+    switch (ACC_RANGE) {
+    case acc_range_3g:
+        _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
+        _px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+        break;
+
+    case acc_range_6g:
+        _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
+        _px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+        break;
+
+    case acc_range_12g:
+        _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
+        _px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+        break;
+
+    case acc_range_24g:
+        _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
+        _px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+        break;
+    }
 }
 
-void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate)
-{
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate) {
+    // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+    const float min_interval = FIFO_SAMPLE_DT;
+    _fifo_empty_interval_us  = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+    _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
 
-	// recompute FIFO empty interval (us) with actual sample limit
-	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+    // recompute FIFO empty interval (us) with actual sample limit
+    _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
 
-	ConfigureFIFOWatermark(_fifo_samples);
+    ConfigureFIFOWatermark(_fifo_samples);
 }
 
-void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO_WTM: 13 bit FIFO watermark level value
-	// unit of the fifo watermark is one byte
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_cfg) {
-		if (r.reg == Register::FIFO_WTM_0) {
-			// fifo_water_mark[7:0]
-			r.set_bits = fifo_watermark_threshold & 0x00FF;
-			r.clear_bits = ~r.set_bits;
-
-		} else if (r.reg == Register::FIFO_WTM_1) {
-			// fifo_water_mark[12:8]
-			r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8;
-			r.clear_bits = ~r.set_bits;
-		}
-	}
+void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples) {
+    // FIFO_WTM: 13 bit FIFO watermark level value
+    // unit of the fifo watermark is one byte
+    const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+    for (auto &r : _register_cfg) {
+        if (r.reg == Register::FIFO_WTM_0) {
+            // fifo_water_mark[7:0]
+            r.set_bits   = fifo_watermark_threshold & 0x00FF;
+            r.clear_bits = ~r.set_bits;
+
+        } else if (r.reg == Register::FIFO_WTM_1) {
+            // fifo_water_mark[12:8]
+            r.set_bits   = (fifo_watermark_threshold & 0x0700) >> 8;
+            r.clear_bits = ~r.set_bits;
+        }
+    }
 }
 
-bool BMI088_Accelerometer::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool BMI088_Accelerometer::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	ConfigureAccel();
+    ConfigureAccel();
 
-	return success;
+    return success;
 }
 
-int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<BMI088_Accelerometer *>(arg)->DataReady();
-	return 0;
+int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<BMI088_Accelerometer *>(arg)->DataReady();
+    return 0;
 }
 
-void BMI088_Accelerometer::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void BMI088_Accelerometer::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool BMI088_Accelerometer::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI088_Accelerometer::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+    // Setup data ready on falling edge
+    return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
-bool BMI088_Accelerometer::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI088_Accelerometer::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+    return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool BMI088_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool BMI088_Accelerometer::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t BMI088_Accelerometer::RegisterRead(Register reg)
-{
-	// 6.1.2 SPI interface of accelerometer part
-	//
-	// In case of read operations of the accelerometer part, the requested data
-	// is not sent immediately, but instead first a dummy byte is sent, and
-	// after this dummy byte the actual requested register content is transmitted.
-	uint8_t cmd[3] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	// cmd[1] dummy byte
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[2];
+uint8_t BMI088_Accelerometer::RegisterRead(Register reg) {
+    // 6.1.2 SPI interface of accelerometer part
+    //
+    // In case of read operations of the accelerometer part, the requested data
+    // is not sent immediately, but instead first a dummy byte is sent, and
+    // after this dummy byte the actual requested register content is transmitted.
+    uint8_t cmd[3]{};
+    cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+    // cmd[1] dummy byte
+    transfer(cmd, cmd, sizeof(cmd));
+    return cmd[2];
 }
 
-void BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	transfer(cmd, cmd, sizeof(cmd));
+void BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t cmd[2]{(uint8_t)reg, value};
+    transfer(cmd, cmd, sizeof(cmd));
 }
 
-void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
+void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
 
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+    uint8_t val = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
 
-uint16_t BMI088_Accelerometer::FIFOReadCount()
-{
-	// FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
-	uint8_t fifo_len_buf[4] {};
-	fifo_len_buf[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ;
-	// fifo_len_buf[1] dummy byte
+uint16_t BMI088_Accelerometer::FIFOReadCount() {
+    // FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
+    uint8_t fifo_len_buf[4]{};
+    fifo_len_buf[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ;
+    // fifo_len_buf[1] dummy byte
 
-	if (transfer(&fifo_len_buf[0], &fifo_len_buf[0], sizeof(fifo_len_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return 0;
-	}
+    if (transfer(&fifo_len_buf[0], &fifo_len_buf[0], sizeof(fifo_len_buf)) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return 0;
+    }
 
-	const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2];        // fifo_byte_counter[7:0]
-	const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
+    const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2];        // fifo_byte_counter[7:0]
+    const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
 
-	return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
+    return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
 }
 
-bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
-
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
-
-	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
-
-	// An empty FIFO corresponds to 0x8000
-	if (fifo_byte_counter == 0x8000) {
-		perf_count(_fifo_empty_perf);
-		return false;
-
-	} else if (fifo_byte_counter >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		return false;
-	}
-
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	// first find all sensor data frames in the buffer
-	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
-	unsigned fifo_buffer_index = 0; // start of buffer
-
-	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
-		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
-		switch (data_buffer[fifo_buffer_index] & 0xFC) {
-		case FIFO::header::sensor_data_frame: {
-				// Acceleration sensor data frame
-				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
-
-				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
-
-				// sensor's frame is +x forward, +y left, +z up
-				//  flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel.x[accel.samples] = accel_x;
-				accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
-
-				fifo_buffer_index += 7; // move forward to next record
-			}
-			break;
-
-		case FIFO::header::skip_frame:
-			// Skip Frame
-			// Frame length: 2 bytes (1 byte header + 1 byte payload)
-			PX4_DEBUG("Skip Frame");
-			fifo_buffer_index += 2;
-			break;
-
-		case FIFO::header::sensor_time_frame:
-			// Sensortime Frame
-			// Frame length: 4 bytes (1 byte header + 3 bytes payload)
-			PX4_DEBUG("Sensortime Frame");
-			fifo_buffer_index += 4;
-			break;
-
-		case FIFO::header::FIFO_input_config_frame:
-			// FIFO input config Frame
-			// Frame length: 2 bytes (1 byte header + 1 byte payload)
-			PX4_DEBUG("FIFO input config Frame");
-			fifo_buffer_index += 2;
-			break;
-
-		case FIFO::header::sample_drop_frame:
-			// Sample drop Frame
-			// Frame length: 2 bytes (1 byte header + 1 byte payload)
-			PX4_DEBUG("Sample drop Frame");
-			fifo_buffer_index += 2;
-			break;
-
-		default:
-			fifo_buffer_index++;
-			break;
-		}
-	}
-
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-		return true;
-	}
-
-	return false;
+bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) {
+    FIFOTransferBuffer buffer{};
+    const size_t       transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+
+    if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return false;
+    }
+
+    const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
+
+    // An empty FIFO corresponds to 0x8000
+    if (fifo_byte_counter == 0x8000) {
+        perf_count(_fifo_empty_perf);
+        return false;
+
+    } else if (fifo_byte_counter >= FIFO::SIZE) {
+        perf_count(_fifo_overflow_perf);
+        return false;
+    }
+
+    sensor_accel_fifo_s accel{};
+    accel.timestamp_sample = timestamp_sample;
+    accel.samples          = 0;
+    accel.dt               = FIFO_SAMPLE_DT;
+
+    // first find all sensor data frames in the buffer
+    uint8_t *data_buffer       = (uint8_t *)&buffer.f[0];
+    unsigned fifo_buffer_index = 0; // start of buffer
+
+    while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
+        // look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
+        switch (data_buffer[fifo_buffer_index] & 0xFC) {
+        case FIFO::header::sensor_data_frame: {
+            // Acceleration sensor data frame
+            // Frame length: 7 bytes (1 byte header + 6 bytes payload)
+
+            FIFO::DATA   *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
+            const int16_t accel_x     = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+            const int16_t accel_y     = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+            const int16_t accel_z     = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+
+            // sensor's frame is +x forward, +y left, +z up
+            //  flip y & z to publish right handed with z down (x forward, y right, z down)
+            accel.x[accel.samples] = accel_x;
+            accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+            accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+            accel.samples++;
+
+            fifo_buffer_index += 7; // move forward to next record
+        } break;
+
+        case FIFO::header::skip_frame:
+            // Skip Frame
+            // Frame length: 2 bytes (1 byte header + 1 byte payload)
+            PX4_DEBUG("Skip Frame");
+            fifo_buffer_index += 2;
+            break;
+
+        case FIFO::header::sensor_time_frame:
+            // Sensortime Frame
+            // Frame length: 4 bytes (1 byte header + 3 bytes payload)
+            PX4_DEBUG("Sensortime Frame");
+            fifo_buffer_index += 4;
+            break;
+
+        case FIFO::header::FIFO_input_config_frame:
+            // FIFO input config Frame
+            // Frame length: 2 bytes (1 byte header + 1 byte payload)
+            PX4_DEBUG("FIFO input config Frame");
+            fifo_buffer_index += 2;
+            break;
+
+        case FIFO::header::sample_drop_frame:
+            // Sample drop Frame
+            // Frame length: 2 bytes (1 byte header + 1 byte payload)
+            PX4_DEBUG("Sample drop Frame");
+            fifo_buffer_index += 2;
+            break;
+
+        default:
+            fifo_buffer_index++;
+            break;
+        }
+    }
+
+    _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+    if (accel.samples > 0) {
+        _px4_accel.updateFIFO(accel);
+        return true;
+    }
+
+    return false;
 }
 
-void BMI088_Accelerometer::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
+void BMI088_Accelerometer::FIFOReset() {
+    perf_count(_fifo_reset_perf);
 
-	// ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E).
-	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
+    // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E).
+    RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
 
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+    // reset while FIFO is disabled
+    _drdy_timestamp_sample.store(0);
 }
 
-void BMI088_Accelerometer::UpdateTemperature()
-{
-	// stored in an 11-bit value in 2’s complement format
-	uint8_t temperature_buf[4] {};
-	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | DIR_READ;
-	// temperature_buf[1] dummy byte
+void BMI088_Accelerometer::UpdateTemperature() {
+    // stored in an 11-bit value in 2’s complement format
+    uint8_t temperature_buf[4]{};
+    temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | DIR_READ;
+    // temperature_buf[1] dummy byte
 
-	if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return;
-	}
+    if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return;
+    }
 
-	const uint8_t TEMP_MSB = temperature_buf[2];
-	const uint8_t TEMP_LSB = temperature_buf[3];
+    const uint8_t TEMP_MSB = temperature_buf[2];
+    const uint8_t TEMP_LSB = temperature_buf[3];
 
-	// Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data
-	uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32);
-	int16_t Temp_int11 = 0;
+    // Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data
+    uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32);
+    int16_t  Temp_int11  = 0;
 
-	if (Temp_uint11 > 1023) {
-		Temp_int11 = Temp_uint11 - 2048;
+    if (Temp_uint11 > 1023) {
+        Temp_int11 = Temp_uint11 - 2048;
 
-	} else {
-		Temp_int11 = Temp_uint11;
-	}
+    } else {
+        Temp_int11 = Temp_uint11;
+    }
 
-	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
+    float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
 
-	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+    if (PX4_ISFINITE(temperature)) {
+        _px4_accel.set_temperature(temperature);
 
-	} else {
-		perf_count(_bad_transfer_perf);
-	}
+    } else {
+        perf_count(_bad_transfer_perf);
+    }
 }
 
 } // namespace Bosch::BMI088::Accelerometer
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp b/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp
index ccf5f40d3a..4298ed4bd3 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088_Accelerometer.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -39,94 +16,93 @@
 
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 
-namespace Bosch::BMI088::Accelerometer
-{
+namespace Bosch::BMI088::Accelerometer {
 
-class BMI088_Accelerometer : public BMI088
-{
+class BMI088_Accelerometer : public BMI088 {
 public:
-	BMI088_Accelerometer(const I2CSPIDriverConfig &config);
-	~BMI088_Accelerometer() override;
+    BMI088_Accelerometer(const I2CSPIDriverConfig &config);
+    ~BMI088_Accelerometer() override;
 
-	void RunImpl() override;
-	void print_status() override;
+    void RunImpl() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr uint32_t RATE{1600}; // 1600 Hz
-	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
-
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ};
-		uint8_t dummy{0};
-		uint8_t FIFO_LENGTH_0{0};
-		uint8_t FIFO_LENGTH_1{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Configure();
-	void ConfigureAccel();
-	void ConfigureSampleRate(int sample_rate = 0);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-
-	uint16_t FIFOReadCount();
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
-	void FIFOReset();
-
-	void UpdateTemperature();
-
-	PX4Accelerometer _px4_accel;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{10};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register                        | Set bits, Clear bits
-		{ Register::ACC_PWR_CONF,          0, ACC_PWR_CONF_BIT::acc_pwr_save },
-		{ Register::ACC_PWR_CTRL,          ACC_PWR_CTRL_BIT::acc_enable, 0 },
-		{ Register::ACC_CONF,              ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 },
-		{ Register::ACC_RANGE,             ACC_RANGE_BIT::acc_range_24g, 0 },
-		{ Register::FIFO_WTM_0,            0, 0 },
-		{ Register::FIFO_WTM_1,            0, 0 },
-		{ Register::FIFO_CONFIG_0,         FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 },
-		{ Register::FIFO_CONFIG_1,         FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 },
-		{ Register::INT1_IO_CONF,          INT1_IO_CONF_BIT::int1_out, 0 },
-		{ Register::INT1_INT2_MAP_DATA,    INT1_INT2_MAP_DATA_BIT::int1_fwm, 0},
-	};
+    void exit_and_cleanup() override;
+
+    // Sensor Configuration
+    static constexpr uint32_t RATE{1600}; // 1600 Hz
+    static constexpr float    FIFO_SAMPLE_DT{1e6f / RATE};
+
+    static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+
+    // Transfer data
+    struct FIFOTransferBuffer {
+        uint8_t    cmd{static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ};
+        uint8_t    dummy{0};
+        uint8_t    FIFO_LENGTH_0{0};
+        uint8_t    FIFO_LENGTH_1{0};
+        FIFO::DATA f[FIFO_MAX_SAMPLES]{};
+    };
+
+    // ensure no struct padding
+    static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Configure();
+    void ConfigureAccel();
+    void ConfigureSampleRate(int sample_rate = 0);
+    void ConfigureFIFOWatermark(uint8_t samples);
+
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
+
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    uint16_t FIFOReadCount();
+    bool     FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+    void     FIFOReset();
+
+    void UpdateTemperature();
+
+    PX4Accelerometer _px4_accel;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: bad transfer")};
+    perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO empty")};
+    perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO overflow")};
+    perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME "_accel: FIFO reset")};
+    perf_counter_t _drdy_missed_perf{nullptr};
+
+    uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{10};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register                        | Set bits, Clear bits
+        {Register::ACC_PWR_CONF, 0, ACC_PWR_CONF_BIT::acc_pwr_save},
+        {Register::ACC_PWR_CTRL, ACC_PWR_CTRL_BIT::acc_enable, 0},
+        {Register::ACC_CONF, ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0},
+        {Register::ACC_RANGE, ACC_RANGE_BIT::acc_range_24g, 0},
+        {Register::FIFO_WTM_0, 0, 0},
+        {Register::FIFO_WTM_1, 0, 0},
+        {Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0},
+        {Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0},
+        {Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0},
+        {Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0},
+    };
 };
 
 } // namespace Bosch::BMI088::Accelerometer
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.cpp b/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.cpp
index d945a815f9..6ad07b24f0 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.cpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.cpp
@@ -1,479 +1,433 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMI088_Gyroscope.hpp"
 
 using namespace time_literals;
 
-namespace Bosch::BMI088::Gyroscope
-{
+namespace Bosch::BMI088::Gyroscope {
 
 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
-	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
-	}
-
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-}
+    BMI088(config),
+    _px4_gyro(get_device_id(), config.rotation) {
+    if (config.drdy_gpio != 0) {
+        _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME "_gyro: DRDY missed");
+    }
 
-BMI088_Gyroscope::~BMI088_Gyroscope()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
+    ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
 }
 
-void BMI088_Gyroscope::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+BMI088_Gyroscope::~BMI088_Gyroscope() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_fifo_empty_perf);
+    perf_free(_fifo_overflow_perf);
+    perf_free(_fifo_reset_perf);
+    perf_free(_drdy_missed_perf);
 }
 
-void BMI088_Gyroscope::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
+void BMI088_Gyroscope::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-int BMI088_Gyroscope::probe()
-{
-	const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID);
+void BMI088_Gyroscope::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	if (chipid != ID) {
-		DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid);
-		return PX4_ERROR;
-	}
+    PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
 
-	return PX4_OK;
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_fifo_empty_perf);
+    perf_print_counter(_fifo_overflow_perf);
+    perf_print_counter(_fifo_reset_perf);
+    perf_print_counter(_drdy_missed_perf);
 }
 
-void BMI088_Gyroscope::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
-		// Following a delay of 30 ms, all configuration settings are overwritten with their reset value.
-		RegisterWrite(Register::GYRO_SOFTRESET, 0xB6);
-		DataReadyInterruptDisable();
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(30_ms);
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) {
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
+int BMI088_Gyroscope::probe() {
+    const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID);
 
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_RESET:
-		// if configure succeeded then start reading from FIFO
-		_state = STATE::FIFO_READ;
-
-		FIFOReset();
-
-		if (DataReadyInterruptConfigure()) {
-			_data_ready_interrupt_enabled = true;
-
-			// backup schedule as a watchdog timeout
-			ScheduleDelayed(100_ms);
-
-		} else {
-			_data_ready_interrupt_enabled = false;
-			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = 0;
-
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			// always check current FIFO status/count
-			bool success = false;
-			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
-
-			if (FIFO_STATUS & FIFO_STATUS_BIT::Fifo_overrun) {
-				FIFOReset();
-				perf_count(_fifo_overflow_perf);
-
-			} else {
-				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::Fifo_frame_counter;
-
-				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
-					// not technically an overflow, but more samples than we expected or can publish
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_frame_counter == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else if (fifo_frame_counter >= 1) {
-
-					uint8_t samples = fifo_frame_counter;
-
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
+    if (chipid != ID) {
+        DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid);
+        return PX4_ERROR;
+    }
 
-					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
-						success = true;
-
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
-					}
-				}
-			}
+    return PX4_OK;
+}
 
-			if (!success) {
-				_failure_count++;
+void BMI088_Gyroscope::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
+        // Following a delay of 30 ms, all configuration settings are overwritten with their reset value.
+        RegisterWrite(Register::GYRO_SOFTRESET, 0xB6);
+        DataReadyInterruptDisable();
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        ScheduleDelayed(30_ms);
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+        if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) {
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then reset the FIFO
+            _state = STATE::FIFO_RESET;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::FIFO_RESET:
+        // if configure succeeded then start reading from FIFO
+        _state = STATE::FIFO_READ;
+
+        FIFOReset();
+
+        if (DataReadyInterruptConfigure()) {
+            _data_ready_interrupt_enabled = true;
+
+            // backup schedule as a watchdog timeout
+            ScheduleDelayed(100_ms);
+
+        } else {
+            _data_ready_interrupt_enabled = false;
+            ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+        }
+
+        break;
+
+    case STATE::FIFO_READ: {
+        hrt_abstime timestamp_sample = 0;
+
+        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) < _fifo_empty_interval_us) {
+                timestamp_sample = drdy_timestamp_sample;
+
+            } else {
+                perf_count(_drdy_missed_perf);
+            }
+
+            // push backup schedule back
+            ScheduleDelayed(_fifo_empty_interval_us * 2);
+        }
+
+        // always check current FIFO status/count
+        bool          success     = false;
+        const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+
+        if (FIFO_STATUS & FIFO_STATUS_BIT::Fifo_overrun) {
+            FIFOReset();
+            perf_count(_fifo_overflow_perf);
+
+        } else {
+            const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::Fifo_frame_counter;
+
+            if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+                // not technically an overflow, but more samples than we expected or can publish
+                FIFOReset();
+                perf_count(_fifo_overflow_perf);
+
+            } else if (fifo_frame_counter == 0) {
+                perf_count(_fifo_empty_perf);
+
+            } else if (fifo_frame_counter >= 1) {
+                uint8_t samples = fifo_frame_counter;
+
+                // tolerate minor jitter, leave sample to next iteration if behind by only 1
+                if (samples == _fifo_samples + 1) {
+                    // sample timestamp set from data ready already corresponds to _fifo_samples
+                    if (timestamp_sample == 0) {
+                        timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+                    }
+
+                    samples--;
+                }
+
+                if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
+                    success = true;
+
+                    if (_failure_count > 0) {
+                        _failure_count--;
+                    }
+                }
+            }
+        }
 
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
 
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
 
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-			}
-		}
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+        }
+    }
 
-		break;
-	}
+    break;
+    }
 }
 
-void BMI088_Gyroscope::ConfigureGyro()
-{
-	const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
-
-	switch (GYRO_RANGE) {
-	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
-		break;
-
-	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
-		break;
-
-	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
-		break;
-
-	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
-		break;
-
-	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
-		break;
-	}
+void BMI088_Gyroscope::ConfigureGyro() {
+    const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
+
+    switch (GYRO_RANGE) {
+    case gyro_range_2000_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 16.384f));
+        _px4_gyro.set_range(math::radians(2000.f));
+        break;
+
+    case gyro_range_1000_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 32.768f));
+        _px4_gyro.set_range(math::radians(1000.f));
+        break;
+
+    case gyro_range_500_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 65.536f));
+        _px4_gyro.set_range(math::radians(500.f));
+        break;
+
+    case gyro_range_250_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 131.072f));
+        _px4_gyro.set_range(math::radians(250.f));
+        break;
+
+    case gyro_range_125_dps:
+        _px4_gyro.set_scale(math::radians(1.f / 262.144f));
+        _px4_gyro.set_range(math::radians(125.f));
+        break;
+    }
 }
 
-void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate)
-{
-	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate) {
+    // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+    const float min_interval = FIFO_SAMPLE_DT;
+    _fifo_empty_interval_us  = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+    _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
 
-	// recompute FIFO empty interval (us) with actual sample limit
-	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+    // recompute FIFO empty interval (us) with actual sample limit
+    _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
 
-	ConfigureFIFOWatermark(_fifo_samples);
+    ConfigureFIFOWatermark(_fifo_samples);
 }
 
-void BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold
-	for (auto &r : _register_cfg) {
-		if (r.reg == Register::FIFO_CONFIG_0) {
-			r.set_bits = samples;
-			r.clear_bits = ~r.set_bits;
-		}
-	}
+void BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples) {
+    // FIFO watermark threshold
+    for (auto &r : _register_cfg) {
+        if (r.reg == Register::FIFO_CONFIG_0) {
+            r.set_bits   = samples;
+            r.clear_bits = ~r.set_bits;
+        }
+    }
 }
 
-bool BMI088_Gyroscope::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool BMI088_Gyroscope::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	ConfigureGyro();
+    ConfigureGyro();
 
-	return success;
+    return success;
 }
 
-int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<BMI088_Gyroscope *>(arg)->DataReady();
-	return 0;
+int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<BMI088_Gyroscope *>(arg)->DataReady();
+    return 0;
 }
 
-void BMI088_Gyroscope::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void BMI088_Gyroscope::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool BMI088_Gyroscope::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI088_Gyroscope::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+    // Setup data ready on falling edge
+    return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
-bool BMI088_Gyroscope::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool BMI088_Gyroscope::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+    return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
-bool BMI088_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool BMI088_Gyroscope::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t BMI088_Gyroscope::RegisterRead(Register reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
+uint8_t BMI088_Gyroscope::RegisterRead(Register reg) {
+    uint8_t cmd[2]{};
+    cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+    transfer(cmd, cmd, sizeof(cmd));
+    return cmd[1];
 }
 
-void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	transfer(cmd, cmd, sizeof(cmd));
+void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t cmd[2]{(uint8_t)reg, value};
+    transfer(cmd, cmd, sizeof(cmd));
 }
 
-void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
+void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
 
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+    uint8_t val = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
 
-bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) {
+    FIFOTransferBuffer buffer{};
+    const size_t       transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
 
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
+    if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return false;
+    }
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+    sensor_gyro_fifo_s gyro{};
+    gyro.timestamp_sample = timestamp_sample;
+    gyro.samples          = samples;
+    gyro.dt               = FIFO_SAMPLE_DT;
 
-	int index = 0;
+    int index = 0;
 
-	for (int i = 0; i < samples; i++) {
-		const FIFO::DATA &fifo_sample = buffer.f[i];
+    for (int i = 0; i < samples; i++) {
+        const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+        const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+        const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+        const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		if (!(gyro_x == INT16_MIN && gyro_y == INT16_MIN && gyro_z == INT16_MIN)) {
-			gyro.x[index] = gyro_x;
-			gyro.y[index] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-			gyro.z[index] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-			++index;
-		}
-	}
+        // sensor's frame is +x forward, +y left, +z up
+        //  flip y & z to publish right handed with z down (x forward, y right, z down)
+        if (!(gyro_x == INT16_MIN && gyro_y == INT16_MIN && gyro_z == INT16_MIN)) {
+            gyro.x[index] = gyro_x;
+            gyro.y[index] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+            gyro.z[index] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+            ++index;
+        }
+    }
 
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+    _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
 
-	if (index > 0) {
-		_px4_gyro.updateFIFO(gyro);
-	}
+    if (index > 0) {
+        _px4_gyro.updateFIFO(gyro);
+    }
 
-	return true;
+    return true;
 }
 
-void BMI088_Gyroscope::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
+void BMI088_Gyroscope::FIFOReset() {
+    perf_count(_fifo_reset_perf);
 
-	// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
-	RegisterWrite(Register::FIFO_CONFIG_0, 0);
+    // FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
+    RegisterWrite(Register::FIFO_CONFIG_0, 0);
 
-	// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
-	RegisterWrite(Register::FIFO_CONFIG_1, 0);
+    // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
+    RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+    // reset while FIFO is disabled
+    _drdy_timestamp_sample.store(0);
 
-	// FIFO_CONFIG_0: restore FIFO watermark
-	// FIFO_CONFIG_1: re-enable FIFO
-	for (const auto &r : _register_cfg) {
-		if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
-			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
-		}
-	}
+    // FIFO_CONFIG_0: restore FIFO watermark
+    // FIFO_CONFIG_1: re-enable FIFO
+    for (const auto &r : _register_cfg) {
+        if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
+            RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+        }
+    }
 }
 
 } // namespace Bosch::BMI088::Gyroscope
diff --git a/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.hpp b/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.hpp
index 59d847bf02..fca662eaa5 100644
--- a/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.hpp
+++ b/apps/peripheral/sensor/imu/bmi088/BMI088_Gyroscope.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -39,86 +16,85 @@
 
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"
 
-namespace Bosch::BMI088::Gyroscope
-{
+namespace Bosch::BMI088::Gyroscope {
 
-class BMI088_Gyroscope : public BMI088
-{
+class BMI088_Gyroscope : public BMI088 {
 public:
-	BMI088_Gyroscope(const I2CSPIDriverConfig &config);
-	~BMI088_Gyroscope() override;
+    BMI088_Gyroscope(const I2CSPIDriverConfig &config);
+    ~BMI088_Gyroscope() override;
 
-	void RunImpl() override;
-	void print_status() override;
+    void RunImpl() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr uint32_t RATE{2000}; // 2000 Hz
-	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
-
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Configure();
-	void ConfigureGyro();
-	void ConfigureSampleRate(int sample_rate = 0);
-	void ConfigureFIFOWatermark(uint8_t samples);
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
-	void FIFOReset();
-
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{8};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register                         | Set bits, Clear bits
-		{ Register::GYRO_RANGE,             GYRO_RANGE_BIT::gyro_range_2000_dps, 0 },
-		{ Register::GYRO_BANDWIDTH,         0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz },
-		{ Register::GYRO_INT_CTRL,          GYRO_INT_CTRL_BIT::fifo_en, 0 },
-		{ Register::INT3_INT4_IO_CONF,      0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl },
-		{ Register::INT3_INT4_IO_MAP,       INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 },
-		{ Register::FIFO_WM_ENABLE,         FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
-		{ Register::FIFO_CONFIG_0,          0, 0 }, // fifo_water_mark_level_trigger_retain<6:0>
-		{ Register::FIFO_CONFIG_1,          FIFO_CONFIG_1_BIT::FIFO_MODE, 0 },
-	};
+    void exit_and_cleanup() override;
+
+    // Sensor Configuration
+    static constexpr uint32_t RATE{2000}; // 2000 Hz
+    static constexpr float    FIFO_SAMPLE_DT{1e6f / RATE};
+
+    static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+
+    // Transfer data
+    struct FIFOTransferBuffer {
+        uint8_t    cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+        FIFO::DATA f[FIFO_MAX_SAMPLES]{};
+    };
+
+    // ensure no struct padding
+    static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Configure();
+    void ConfigureGyro();
+    void ConfigureSampleRate(int sample_rate = 0);
+    void ConfigureFIFOWatermark(uint8_t samples);
+
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
+
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+    void FIFOReset();
+
+    PX4Gyroscope _px4_gyro;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: bad transfer")};
+    perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO empty")};
+    perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO overflow")};
+    perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME "_gyro: FIFO reset")};
+    perf_counter_t _drdy_missed_perf{nullptr};
+
+    uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{8};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register                         | Set bits, Clear bits
+        {Register::GYRO_RANGE, GYRO_RANGE_BIT::gyro_range_2000_dps, 0},
+        {Register::GYRO_BANDWIDTH, 0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz},
+        {Register::GYRO_INT_CTRL, GYRO_INT_CTRL_BIT::fifo_en, 0},
+        {Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl},
+        {Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0},
+        {Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0},
+        {Register::FIFO_CONFIG_0, 0, 0}, // fifo_water_mark_level_trigger_retain<6:0>
+        {Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::FIFO_MODE, 0},
+    };
 };
 
 } // namespace Bosch::BMI088::Gyroscope
diff --git a/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp b/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
index 00b8ffe6d6..3bbc64614d 100644
--- a/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
+++ b/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
@@ -1,40 +1,16 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
-namespace Bosch::BMI088::Accelerometer
-{
+namespace Bosch::BMI088::Accelerometer {
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -47,99 +23,98 @@ static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
-static constexpr uint8_t DIR_READ = 0x80;
+static constexpr uint8_t  DIR_READ  = 0x80;
 
-static constexpr uint8_t ID_088 = 0x1E;
+static constexpr uint8_t ID_088  = 0x1E;
 static constexpr uint8_t ID_090L = 0x1A;
 
 enum class Register : uint8_t {
-	ACC_CHIP_ID        = 0x00,
+    ACC_CHIP_ID = 0x00,
 
-	TEMP_MSB           = 0x22,
-	TEMP_LSB           = 0x23,
+    TEMP_MSB = 0x22,
+    TEMP_LSB = 0x23,
 
-	FIFO_LENGTH_0      = 0x24,
-	FIFO_LENGTH_1      = 0x25,
+    FIFO_LENGTH_0 = 0x24,
+    FIFO_LENGTH_1 = 0x25,
 
-	FIFO_DATA          = 0x26,
+    FIFO_DATA = 0x26,
 
-	ACC_CONF           = 0x40,
-	ACC_RANGE          = 0x41,
+    ACC_CONF  = 0x40,
+    ACC_RANGE = 0x41,
 
-	FIFO_WTM_0         = 0x46,
-	FIFO_WTM_1         = 0x47,
-	FIFO_CONFIG_0      = 0x48,
-	FIFO_CONFIG_1      = 0x49,
+    FIFO_WTM_0    = 0x46,
+    FIFO_WTM_1    = 0x47,
+    FIFO_CONFIG_0 = 0x48,
+    FIFO_CONFIG_1 = 0x49,
 
-	INT1_IO_CONF       = 0x53,
+    INT1_IO_CONF = 0x53,
 
-	INT1_INT2_MAP_DATA = 0x58,
+    INT1_INT2_MAP_DATA = 0x58,
 
-	ACC_PWR_CONF       = 0x7C,
-	ACC_PWR_CTRL       = 0x7D,
-	ACC_SOFTRESET      = 0x7E,
+    ACC_PWR_CONF  = 0x7C,
+    ACC_PWR_CTRL  = 0x7D,
+    ACC_SOFTRESET = 0x7E,
 };
 
 // ACC_CONF
 enum ACC_CONF_BIT : uint8_t {
-	// [7:4] acc_bwp
-	acc_bwp_Normal = Bit7 | Bit5,        // Filter setting normal
+    // [7:4] acc_bwp
+    acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal
 
-	// [3:0] acc_odr
-	acc_odr_1600   = Bit3 | Bit2,        // ODR 1600 Hz
+    // [3:0] acc_odr
+    acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz
 };
 
 // ACC_RANGE
 enum ACC_RANGE_BIT : uint8_t {
-	acc_range_3g  = 0,           // ±3g
-	acc_range_6g  = Bit0,        // ±6g
-	acc_range_12g = Bit1,        // ±12g
-	acc_range_24g = Bit1 | Bit0, // ±24g
+    acc_range_3g  = 0,           // ±3g
+    acc_range_6g  = Bit0,        // ±6g
+    acc_range_12g = Bit1,        // ±12g
+    acc_range_24g = Bit1 | Bit0, // ±24g
 };
 
 // FIFO_CONFIG_0
 enum FIFO_CONFIG_0_BIT : uint8_t {
-	BIT1_ALWAYS = Bit1, // This bit must always be ‘1’.
-	FIFO_mode   = Bit0,
+    BIT1_ALWAYS = Bit1, // This bit must always be ‘1’.
+    FIFO_mode   = Bit0,
 };
 
 // FIFO_CONFIG_1
 enum FIFO_CONFIG_1_BIT : uint8_t {
-	Acc_en      = Bit6,
-	BIT4_ALWAYS = Bit4, // This bit must always be ‘1’.
+    Acc_en      = Bit6,
+    BIT4_ALWAYS = Bit4, // This bit must always be ‘1’.
 };
 
 // INT1_IO_CONF
 enum INT1_IO_CONF_BIT : uint8_t {
-	int1_in  = Bit4,
-	int1_out = Bit3,
+    int1_in  = Bit4,
+    int1_out = Bit3,
 
-	int1_lvl = Bit1,
+    int1_lvl = Bit1,
 };
 
 // INT1_INT2_MAP_DATA
 enum INT1_INT2_MAP_DATA_BIT : uint8_t {
-	int2_drdy  = Bit6,
-	int2_fwm   = Bit5,
-	int2_ffull = Bit4,
+    int2_drdy  = Bit6,
+    int2_fwm   = Bit5,
+    int2_ffull = Bit4,
 
-	int1_drdy  = Bit2,
-	int1_fwm   = Bit1,
-	int1_ffull = Bit0,
+    int1_drdy  = Bit2,
+    int1_fwm   = Bit1,
+    int1_ffull = Bit0,
 };
 
 // ACC_PWR_CONF
 enum ACC_PWR_CONF_BIT : uint8_t {
-	acc_pwr_save = 0x03
+    acc_pwr_save = 0x03
 };
 
 // ACC_PWR_CTRL
 enum ACC_PWR_CTRL_BIT : uint8_t {
-	acc_enable = 0x04
+    acc_enable = 0x04
 };
 
-namespace FIFO
-{
+namespace FIFO {
 static constexpr size_t SIZE = 1024;
 
 // 1. Acceleration sensor data frame   - Frame length: 7 bytes (1 byte header + 6 bytes payload)
@@ -150,22 +125,23 @@ static constexpr size_t SIZE = 1024;
 // Payload: Sensortime (content of registers 0x18 – 0x1A), taken when the last byte of the last frame is read.
 
 struct DATA {
-	uint8_t Header;
-	uint8_t ACC_X_LSB;
-	uint8_t ACC_X_MSB;
-	uint8_t ACC_Y_LSB;
-	uint8_t ACC_Y_MSB;
-	uint8_t ACC_Z_LSB;
-	uint8_t ACC_Z_MSB;
+    uint8_t Header;
+    uint8_t ACC_X_LSB;
+    uint8_t ACC_X_MSB;
+    uint8_t ACC_Y_LSB;
+    uint8_t ACC_Y_MSB;
+    uint8_t ACC_Z_LSB;
+    uint8_t ACC_Z_MSB;
 };
+
 static_assert(sizeof(DATA) == 7);
 
 enum header : uint8_t {
-	sensor_data_frame       = 0b10000100,
-	skip_frame              = 0b01000000,
-	sensor_time_frame       = 0b01000100,
-	FIFO_input_config_frame = 0b01001000,
-	sample_drop_frame       = 0b01010000,
+    sensor_data_frame       = 0b10000100,
+    skip_frame              = 0b01000000,
+    sensor_time_frame       = 0b01000100,
+    FIFO_input_config_frame = 0b01001000,
+    sample_drop_frame       = 0b01010000,
 };
 
 } // namespace FIFO
diff --git a/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp b/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
index 0f1974f2cb..795f9ffe86 100644
--- a/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
+++ b/apps/peripheral/sensor/imu/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
@@ -1,40 +1,16 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
-namespace Bosch::BMI088::Gyroscope
-{
+namespace Bosch::BMI088::Gyroscope {
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -47,91 +23,90 @@ static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
-static constexpr uint8_t DIR_READ = 0x80;
+static constexpr uint8_t  DIR_READ  = 0x80;
 
 static constexpr uint8_t ID = 0x0F;
 
 enum class Register : uint8_t {
-	GYRO_CHIP_ID      = 0x00,
+    GYRO_CHIP_ID = 0x00,
 
-	FIFO_STATUS       = 0x0E,
-	GYRO_RANGE        = 0x0F,
-	GYRO_BANDWIDTH    = 0x10,
+    FIFO_STATUS    = 0x0E,
+    GYRO_RANGE     = 0x0F,
+    GYRO_BANDWIDTH = 0x10,
 
-	GYRO_SOFTRESET    = 0x14,
-	GYRO_INT_CTRL     = 0x15,
-	INT3_INT4_IO_CONF = 0x16,
-	INT3_INT4_IO_MAP  = 0x18,
+    GYRO_SOFTRESET    = 0x14,
+    GYRO_INT_CTRL     = 0x15,
+    INT3_INT4_IO_CONF = 0x16,
+    INT3_INT4_IO_MAP  = 0x18,
 
-	FIFO_WM_ENABLE    = 0x1E,
+    FIFO_WM_ENABLE = 0x1E,
 
-	FIFO_CONFIG_0     = 0x3D,
-	FIFO_CONFIG_1     = 0x3E,
-	FIFO_DATA         = 0x3F,
+    FIFO_CONFIG_0 = 0x3D,
+    FIFO_CONFIG_1 = 0x3E,
+    FIFO_DATA     = 0x3F,
 };
 
 // FIFO_STATUS
 enum FIFO_STATUS_BIT : uint8_t {
-	Fifo_overrun       = Bit7,
-	Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
+    Fifo_overrun       = Bit7,
+    Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
 };
 
 // GYRO_RANGE
 enum GYRO_RANGE_BIT : uint8_t {
-	gyro_range_2000_dps = 0x00, // ±2000
-	gyro_range_1000_dps = 0x01, // ±1000
-	gyro_range_500_dps  = 0x02, // ±500
-	gyro_range_250_dps  = 0x04, // ±250
-	gyro_range_125_dps  = 0x05, // ±125
+    gyro_range_2000_dps = 0x00, // ±2000
+    gyro_range_1000_dps = 0x01, // ±1000
+    gyro_range_500_dps  = 0x02, // ±500
+    gyro_range_250_dps  = 0x04, // ±250
+    gyro_range_125_dps  = 0x05, // ±125
 };
 
 // GYRO_BANDWIDTH
 enum GYRO_BANDWIDTH_BIT : uint8_t {
-	gyro_bw_532_Hz = Bit2 | Bit1 | Bit0
+    gyro_bw_532_Hz = Bit2 | Bit1 | Bit0
 };
 
 // GYRO_INT_CTRL
 enum GYRO_INT_CTRL_BIT : uint8_t {
-	data_en = Bit7,
-	fifo_en = Bit6,
+    data_en = Bit7,
+    fifo_en = Bit6,
 };
 
 // INT3_INT4_IO_CONF
 enum INT3_INT4_IO_CONF_BIT : uint8_t {
-	Int3_od  = Bit1, // ‘0’ Push-pull
-	Int3_lvl = Bit0, // ‘0’ Active low
+    Int3_od  = Bit1, // ‘0’ Push-pull
+    Int3_lvl = Bit0, // ‘0’ Active low
 };
 
 // INT3_INT4_IO_MAP
 enum INT3_INT4_IO_MAP_BIT : uint8_t {
-	Int4_data = Bit7,
-	Int4_fifo = Bit5,
-	Int3_fifo = Bit2,
-	Int3_data = Bit0,
+    Int4_data = Bit7,
+    Int4_fifo = Bit5,
+    Int3_fifo = Bit2,
+    Int3_data = Bit0,
 };
 
 // FIFO_WM_ENABLE
 enum FIFO_WM_ENABLE_BIT : uint8_t {
-	fifo_wm_enable  = Bit7 | Bit3,
-	fifo_wm_disable = Bit3,
+    fifo_wm_enable  = Bit7 | Bit3,
+    fifo_wm_disable = Bit3,
 };
 
 // FIFO_CONFIG_1
 enum FIFO_CONFIG_1_BIT : uint8_t {
-	FIFO_MODE = Bit6,
+    FIFO_MODE = Bit6,
 };
 
-
-namespace FIFO
-{
+namespace FIFO {
 struct DATA {
-	uint8_t RATE_X_LSB;
-	uint8_t RATE_X_MSB;
-	uint8_t RATE_Y_LSB;
-	uint8_t RATE_Y_MSB;
-	uint8_t RATE_Z_LSB;
-	uint8_t RATE_Z_MSB;
+    uint8_t RATE_X_LSB;
+    uint8_t RATE_X_MSB;
+    uint8_t RATE_Y_LSB;
+    uint8_t RATE_Y_MSB;
+    uint8_t RATE_Z_LSB;
+    uint8_t RATE_Z_MSB;
 };
+
 static_assert(sizeof(DATA) == 6);
 
 // 100 frames of data in FIFO mode
diff --git a/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp b/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp
index 182c1e90d4..2e3d95046f 100644
--- a/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp
+++ b/apps/peripheral/sensor/imu/bmi088/bmi088_main.cpp
@@ -1,101 +1,76 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
 #include "BMI088.hpp"
 
-void BMI088::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("bmi088", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
-	PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void BMI088::print_usage() {
+    PRINT_MODULE_USAGE_NAME("bmi088", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
+    PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int bmi088_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = BMI088;
-	BusCLIArguments cli{false, true};
-	uint16_t type = 0;
-	cli.default_spi_frequency = 10000000;
-	const char *name = MODULE_NAME;
+extern "C" int bmi088_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = BMI088;
+    BusCLIArguments cli{false, true};
+    uint16_t        type      = 0;
+    cli.default_spi_frequency = 10000000;
+    const char *name          = MODULE_NAME;
 
-	while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
-		switch (ch) {
-		case 'A':
-			type = DRV_ACC_DEVTYPE_BMI088;
-			name = MODULE_NAME "_accel";
-			break;
+    while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
+        switch (ch) {
+        case 'A':
+            type = DRV_ACC_DEVTYPE_BMI088;
+            name = MODULE_NAME "_accel";
+            break;
 
-		case 'G':
-			type = DRV_GYR_DEVTYPE_BMI088;
-			name = MODULE_NAME "_gyro";
-			break;
+        case 'G':
+            type = DRV_GYR_DEVTYPE_BMI088;
+            name = MODULE_NAME "_gyro";
+            break;
 
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb || type == 0) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb || type == 0) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(name, cli, type);
+    BusInstanceIterator iterator(name, cli, type);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/imu/icm42688p/ICM42688P.cpp b/apps/peripheral/sensor/imu/icm42688p/ICM42688P.cpp
index b123768e0b..0319bd1561 100644
--- a/apps/peripheral/sensor/imu/icm42688p/ICM42688P.cpp
+++ b/apps/peripheral/sensor/imu/icm42688p/ICM42688P.cpp
@@ -1,894 +1,833 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "ICM42688P.hpp"
 
 using namespace time_literals;
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
 }
 
-static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
+static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
 }
 
 ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
-	SPI(config),
-	I2CSPIDriver(config),
-	_drdy_gpio(config.drdy_gpio),
-	_px4_accel(get_device_id(), config.rotation),
-	_px4_gyro(get_device_id(), config.rotation)
-{
-	isICM686 = config.custom2 == DRV_IMU_DEVTYPE_ICM42686P;
-
-	if (config.drdy_gpio != 0) {
-		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
-	}
-
-	if (config.custom1 != 0) {
-		_enable_clock_input = true;
-		_input_clock_freq = config.custom1;
-		ConfigureCLKIN();
-
-	} else {
-		_enable_clock_input = false;
-	}
-
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+    SPI(config),
+    I2CSPIDriver(config),
+    _drdy_gpio(config.drdy_gpio),
+    _px4_accel(get_device_id(), config.rotation),
+    _px4_gyro(get_device_id(), config.rotation) {
+    isICM686 = config.custom2 == DRV_IMU_DEVTYPE_ICM42686P;
+
+    if (config.drdy_gpio != 0) {
+        _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME ": DRDY missed");
+    }
+
+    if (config.custom1 != 0) {
+        _enable_clock_input = true;
+        _input_clock_freq   = config.custom1;
+        ConfigureCLKIN();
+
+    } else {
+        _enable_clock_input = false;
+    }
+
+    ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
 }
 
-ICM42688P::~ICM42688P()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_fifo_empty_perf);
-	perf_free(_fifo_overflow_perf);
-	perf_free(_fifo_reset_perf);
-	perf_free(_drdy_missed_perf);
+ICM42688P::~ICM42688P() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_fifo_empty_perf);
+    perf_free(_fifo_overflow_perf);
+    perf_free(_fifo_reset_perf);
+    perf_free(_drdy_missed_perf);
 }
 
-int ICM42688P::init()
-{
-	int ret = SPI::init();
+int ICM42688P::init() {
+    int ret = SPI::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("SPI::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool ICM42688P::Reset()
-{
-	_state = STATE::RESET;
-	DataReadyInterruptDisable();
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool ICM42688P::Reset() {
+    _state = STATE::RESET;
+    DataReadyInterruptDisable();
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
 
-void ICM42688P::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+void ICM42688P::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void ICM42688P::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void ICM42688P::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
-	PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
+    PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+    PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
 
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_missed_perf);
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_fifo_empty_perf);
+    perf_print_counter(_fifo_overflow_perf);
+    perf_print_counter(_fifo_reset_perf);
+    perf_print_counter(_drdy_missed_perf);
 }
 
-int ICM42688P::probe()
-{
-	for (int i = 0; i < 3; i++) {
-		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+int ICM42688P::probe() {
+    for (int i = 0; i < 3; i++) {
+        uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
 
-		if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
-			return PX4_OK;
+        if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
+            return PX4_OK;
 
-		} else {
-			DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
+        } else {
+            DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
 
-			uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
-			int bank = reg_bank_sel >> 4;
+            uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
+            int     bank         = reg_bank_sel >> 4;
 
-			if (bank >= 1 && bank <= 3) {
-				DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
-				// force bank selection and retry
-				SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true);
-			}
-		}
-	}
+            if (bank >= 1 && bank <= 3) {
+                DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
+                // force bank selection and retry
+                SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true);
+            }
+        }
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void ICM42688P::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
-		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if (((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI || (isICM686
-				&& RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI686)))
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
-		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
-
-			// Wakeup accel and gyro and schedule remaining configuration
-			RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(1_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::FIFO_RESET:
-
-		_state = STATE::FIFO_READ;
-		FIFOReset();
-
-		if (DataReadyInterruptConfigure()) {
-			_data_ready_interrupt_enabled = true;
-
-			// backup schedule as a watchdog timeout
-			ScheduleDelayed(100_ms);
-
-		} else {
-			_data_ready_interrupt_enabled = false;
-			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-		}
-
-		break;
-
-	case STATE::FIFO_READ: {
-			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
-
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-					samples = _fifo_gyro_samples;
-
-				} else {
-					perf_count(_drdy_missed_perf);
-				}
-
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
-
-				if (fifo_count >= FIFO::SIZE) {
-					FIFOReset();
-					perf_count(_fifo_overflow_perf);
-
-				} else if (fifo_count == 0) {
-					perf_count(_fifo_empty_perf);
-
-				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
-
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
-
-					if (samples > FIFO_MAX_SAMPLES) {
-						// not technically an overflow, but more samples than we expected or can publish
-						FIFOReset();
-						perf_count(_fifo_overflow_perf);
-						samples = 0;
-					}
-				}
-			}
-
-			bool success = false;
-
-			if (samples >= 1) {
-				if (FIFORead(timestamp_sample, samples)) {
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-				   ) {
-					_last_config_check_timestamp = now;
-					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-			}
-		}
-
-		break;
-	}
+void ICM42688P::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // DEVICE_CONFIG: Software reset configuration
+        RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+        if (((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI || (isICM686 && RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI686)))
+            && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
+            && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
+            // Wakeup accel and gyro and schedule remaining configuration
+            RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then reset the FIFO
+            _state = STATE::FIFO_RESET;
+            ScheduleDelayed(1_ms);
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::FIFO_RESET:
+
+        _state = STATE::FIFO_READ;
+        FIFOReset();
+
+        if (DataReadyInterruptConfigure()) {
+            _data_ready_interrupt_enabled = true;
+
+            // backup schedule as a watchdog timeout
+            ScheduleDelayed(100_ms);
+
+        } else {
+            _data_ready_interrupt_enabled = false;
+            ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+        }
+
+        break;
+
+    case STATE::FIFO_READ: {
+        hrt_abstime timestamp_sample = now;
+        uint8_t     samples          = 0;
+
+        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) < _fifo_empty_interval_us) {
+                timestamp_sample = drdy_timestamp_sample;
+                samples          = _fifo_gyro_samples;
+
+            } else {
+                perf_count(_drdy_missed_perf);
+            }
+
+            // push backup schedule back
+            ScheduleDelayed(_fifo_empty_interval_us * 2);
+        }
+
+        if (samples == 0) {
+            // check current FIFO count
+            const uint16_t fifo_count = FIFOReadCount();
+
+            if (fifo_count >= FIFO::SIZE) {
+                FIFOReset();
+                perf_count(_fifo_overflow_perf);
+
+            } else if (fifo_count == 0) {
+                perf_count(_fifo_empty_perf);
+
+            } else {
+                // FIFO count (size in bytes)
+                samples = (fifo_count / sizeof(FIFO::DATA));
+
+                // tolerate minor jitter, leave sample to next iteration if behind by only 1
+                if (samples == _fifo_gyro_samples + 1) {
+                    timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+                    samples--;
+                }
+
+                if (samples > FIFO_MAX_SAMPLES) {
+                    // not technically an overflow, but more samples than we expected or can publish
+                    FIFOReset();
+                    perf_count(_fifo_overflow_perf);
+                    samples = 0;
+                }
+            }
+        }
+
+        bool success = false;
+
+        if (samples >= 1) {
+            if (FIFORead(timestamp_sample, samples)) {
+                success = true;
+
+                if (_failure_count > 0) {
+                    _failure_count--;
+                }
+            }
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+                && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
+                && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])) {
+                _last_config_check_timestamp = now;
+                _checked_register_bank0      = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+                _checked_register_bank1      = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+                _checked_register_bank2      = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+        }
+    }
+
+    break;
+    }
 }
 
-void ICM42688P::ConfigureSampleRate(int sample_rate)
-{
-	// round down to nearest FIFO sample dt
-	const float min_interval = FIFO_SAMPLE_DT;
-	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
+void ICM42688P::ConfigureSampleRate(int sample_rate) {
+    // round down to nearest FIFO sample dt
+    const float min_interval = FIFO_SAMPLE_DT;
+    _fifo_empty_interval_us  = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
 
-	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
+    _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
 
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
+    // recompute FIFO empty interval (us) with actual gyro sample limit
+    _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
-	ConfigureFIFOWatermark(_fifo_gyro_samples);
+    ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
 
-void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
-{
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_bank0_cfg) {
-		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
-			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
-
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
-		}
-	}
+void ICM42688P::ConfigureFIFOWatermark(uint8_t samples) {
+    // FIFO watermark threshold in number of bytes
+    const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+    for (auto &r : _register_bank0_cfg) {
+        if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
+            // FIFO_WM[7:0]  FIFO_CONFIG2
+            r.set_bits = fifo_watermark_threshold & 0xFF;
+
+        } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+            // FIFO_WM[11:8] FIFO_CONFIG3
+            r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
+        }
+    }
 }
 
-void ICM42688P::ConfigureCLKIN()
-{
-	for (auto &r0 : _register_bank0_cfg) {
-		if (r0.reg == Register::BANK_0::INTF_CONFIG1) {
-			r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
-		}
-	}
-
-	for (auto &r1 : _register_bank1_cfg) {
-		if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
-			r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
-			r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
-		}
-	}
+void ICM42688P::ConfigureCLKIN() {
+    for (auto &r0 : _register_bank0_cfg) {
+        if (r0.reg == Register::BANK_0::INTF_CONFIG1) {
+            r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
+        }
+    }
+
+    for (auto &r1 : _register_bank1_cfg) {
+        if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
+            r1.set_bits   = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
+            r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
+        }
+    }
 }
 
-void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
-{
-	if (bank != _last_register_bank || force) {
-		// select BANK_0
-		uint8_t cmd_bank_sel[2] {};
-		cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
-		cmd_bank_sel[1] = bank;
-		transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
-
-		_last_register_bank = bank;
-	}
+void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) {
+    if (bank != _last_register_bank || force) {
+        // select BANK_0
+        uint8_t cmd_bank_sel[2]{};
+        cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
+        cmd_bank_sel[1] = bank;
+        transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
+
+        _last_register_bank = bank;
+    }
 }
 
-bool ICM42688P::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
-
-	// now check that all are configured
-	bool success = true;
-
-	for (const auto &reg_cfg : _register_bank0_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank1_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	for (const auto &reg_cfg : _register_bank2_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
-
-	// 20-bits data format used
-	//  For the 688 the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
-	//  For the 686 the only FSR settings that are operational are ±4000dps for gyroscope and ±32g for accelerometer
-
-	if (isICM686) {
-		_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
-		_px4_gyro.set_range(math::radians(4000.f));
-
-	} else {
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		_px4_gyro.set_range(math::radians(2000.f));
-	}
-
-	return success;
+bool ICM42688P::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_bank0_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
+
+    for (const auto &reg_cfg : _register_bank1_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
+
+    for (const auto &reg_cfg : _register_bank2_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
+
+    // now check that all are configured
+    bool success = true;
+
+    for (const auto &reg_cfg : _register_bank0_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
+
+    for (const auto &reg_cfg : _register_bank1_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
+
+    for (const auto &reg_cfg : _register_bank2_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
+
+    // 20-bits data format used
+    //  For the 688 the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+    //  For the 686 the only FSR settings that are operational are ±4000dps for gyroscope and ±32g for accelerometer
+
+    if (isICM686) {
+        _px4_accel.set_range(32.f * CONSTANTS_ONE_G);
+        _px4_gyro.set_range(math::radians(4000.f));
+
+    } else {
+        _px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+        _px4_gyro.set_range(math::radians(2000.f));
+    }
+
+    return success;
 }
 
-int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<ICM42688P *>(arg)->DataReady();
-	return 0;
+int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<ICM42688P *>(arg)->DataReady();
+    return 0;
 }
 
-void ICM42688P::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void ICM42688P::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool ICM42688P::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool ICM42688P::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+    // Setup data ready on falling edge
+    return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
-bool ICM42688P::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool ICM42688P::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+    return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }
 
 template <typename T>
-bool ICM42688P::RegisterCheck(const T &reg_cfg)
-{
-	bool success = true;
+bool ICM42688P::RegisterCheck(const T &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
 template <typename T>
-uint8_t ICM42688P::RegisterRead(T reg)
-{
-	uint8_t cmd[2] {};
-	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
-	return cmd[1];
+uint8_t ICM42688P::RegisterRead(T reg) {
+    uint8_t cmd[2]{};
+    cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+    SelectRegisterBank(reg);
+    transfer(cmd, cmd, sizeof(cmd));
+    return cmd[1];
 }
 
 template <typename T>
-void ICM42688P::RegisterWrite(T reg, uint8_t value)
-{
-	uint8_t cmd[2] { (uint8_t)reg, value };
-	SelectRegisterBank(reg);
-	transfer(cmd, cmd, sizeof(cmd));
+void ICM42688P::RegisterWrite(T reg, uint8_t value) {
+    uint8_t cmd[2]{(uint8_t)reg, value};
+    SelectRegisterBank(reg);
+    transfer(cmd, cmd, sizeof(cmd));
 }
 
 template <typename T>
-void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
+void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
 
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+    uint8_t val = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
 
-uint16_t ICM42688P::FIFOReadCount()
-{
-	// read FIFO count
-	uint8_t fifo_count_buf[3] {};
-	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+uint16_t ICM42688P::FIFOReadCount() {
+    // read FIFO count
+    uint8_t fifo_count_buf[3]{};
+    fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
+    SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
 
-	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return 0;
-	}
+    if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return 0;
+    }
 
-	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+    return combine(fifo_count_buf[1], fifo_count_buf[2]);
 }
 
-bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
-{
-	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
-	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
-
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
-	}
-
-	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
-	}
-
-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
-	// check FIFO header in every sample
-	uint8_t valid_samples = 0;
-
-	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-		bool valid = true;
-
-		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
-
-		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
-			// FIFO sample empty if HEADER_MSG set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
-			// accel bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
-			// gyro bit not set
-			valid = false;
-
-		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
-			// Packet does not contain a new and valid extended 20-bit data
-			valid = false;
-
-		} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
-			// Packet does not contain ODR timestamp
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
-			// accel ODR changed
-			valid = false;
-
-		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
-			// gyro ODR changed
-			valid = false;
-		}
-
-		if (valid) {
-			valid_samples++;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-			break;
-		}
-	}
-
-	if (valid_samples > 0) {
-		if (ProcessTemperature(buffer.f, valid_samples)) {
-			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
-			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
-			return true;
-		}
-	}
-
-	return false;
+bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples) {
+    FIFOTransferBuffer buffer{};
+    const size_t       transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+    SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+
+    if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+        perf_count(_bad_transfer_perf);
+        return false;
+    }
+
+    if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+        perf_count(_fifo_overflow_perf);
+        FIFOReset();
+        return false;
+    }
+
+    const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+    if (fifo_count_bytes >= FIFO::SIZE) {
+        perf_count(_fifo_overflow_perf);
+        FIFOReset();
+        return false;
+    }
+
+    const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+
+    if (fifo_count_samples == 0) {
+        perf_count(_fifo_empty_perf);
+        return false;
+    }
+
+    // check FIFO header in every sample
+    uint8_t valid_samples = 0;
+
+    for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+        bool valid = true;
+
+        // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+        const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+        if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+            // FIFO sample empty if HEADER_MSG set
+            valid = false;
+
+        } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+            // accel bit not set
+            valid = false;
+
+        } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+            // gyro bit not set
+            valid = false;
+
+        } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+            // Packet does not contain a new and valid extended 20-bit data
+            valid = false;
+
+        } else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
+            // Packet does not contain ODR timestamp
+            valid = false;
+
+        } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+            // accel ODR changed
+            valid = false;
+
+        } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+            // gyro ODR changed
+            valid = false;
+        }
+
+        if (valid) {
+            valid_samples++;
+
+        } else {
+            perf_count(_bad_transfer_perf);
+            break;
+        }
+    }
+
+    if (valid_samples > 0) {
+        if (ProcessTemperature(buffer.f, valid_samples)) {
+            ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+            ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+            return true;
+        }
+    }
+
+    return false;
 }
 
-void ICM42688P::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
+void ICM42688P::FIFOReset() {
+    perf_count(_fifo_reset_perf);
 
-	// SIGNAL_PATH_RESET: FIFO flush
-	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+    // SIGNAL_PATH_RESET: FIFO flush
+    RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
+    // reset while FIFO is disabled
+    _drdy_timestamp_sample.store(0);
 }
 
-static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
-{
-	// 0xXXXAABBC
-	uint32_t high   = ((a << 12) & 0x000FF000);
-	uint32_t low    = ((b << 4)  & 0x00000FF0);
-	uint32_t lowest = (c         & 0x0000000F);
+static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c) {
+    // 0xXXXAABBC
+    uint32_t high   = ((a << 12) & 0x000FF000);
+    uint32_t low    = ((b << 4) & 0x00000FF0);
+    uint32_t lowest = (c & 0x0000000F);
 
-	uint32_t x = high | low | lowest;
+    uint32_t x = high | low | lowest;
 
-	if (a & Bit7) {
-		// sign extend
-		x |= 0xFFF00000u;
-	}
+    if (a & Bit7) {
+        // sign extend
+        x |= 0xFFF00000u;
+    }
 
-	return static_cast<int32_t>(x);
+    return static_cast<int32_t>(x);
 }
 
-void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-
-	// 18-bits of accelerometer data, sent as 20-bits, 2 least significant bits always 0
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-
-		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
-
-		if (_enable_clock_input) {
-			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
-
-		} else {
-			accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
-		}
-
-		// 20 bit hires mode
-		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-		// Accel data is 18 bit ()
-		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
-						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
-						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
-						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-		// sample invalid if -524288
-		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
-			// check if any values are going to exceed int16 limits
-			static constexpr int16_t max_accel = INT16_MAX;
-			static constexpr int16_t min_accel = INT16_MIN;
-
-			if (accel_x >= max_accel || accel_x <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_y >= max_accel || accel_y <= min_accel) {
-				scale_20bit = true;
-			}
-
-			if (accel_z >= max_accel || accel_z <= min_accel) {
-				scale_20bit = true;
-			}
-
-			// shift by 2 (2 least significant bits are always 0)
-			accel.x[accel.samples] = accel_x / 4;
-			accel.y[accel.samples] = accel_y / 4;
-			accel.z[accel.samples] = accel_z / 4;
-			accel.samples++;
-		}
-	}
-
-	if (!scale_20bit) {
-		// On the 686, if highres enabled accel data is always 4096 LSB/g
-		// On the 688, if highres enabled accel data is always 8192 LSB/g
-		if (isICM686) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-
-		} else {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			// 20 bit hires mode
-			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-			// Accel data is 18 bit ()
-			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
-			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
-			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
-
-			accel.x[i] = accel_x;
-			accel.y[i] = accel_y;
-			accel.z[i] = accel_z;
-		}
-
-		if (isICM686) {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
-
-		} else {
-			_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		}
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < accel.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel.x[i];
-		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
-		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
-	}
-
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
+void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples) {
+    sensor_accel_fifo_s accel{};
+    accel.timestamp_sample = timestamp_sample;
+    accel.samples          = 0;
+
+    // 18-bits of accelerometer data, sent as 20-bits, 2 least significant bits always 0
+    bool scale_20bit = false;
+
+    // first pass
+    for (int i = 0; i < samples; i++) {
+        uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+        if (_enable_clock_input) {
+            accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+        } else {
+            accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+        }
+
+        // 20 bit hires mode
+        // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+        // Accel data is 18 bit ()
+        int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+                                           fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+        int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+                                           fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+        int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+                                           fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+        // sample invalid if -524288
+        if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+            // check if any values are going to exceed int16 limits
+            static constexpr int16_t max_accel = INT16_MAX;
+            static constexpr int16_t min_accel = INT16_MIN;
+
+            if (accel_x >= max_accel || accel_x <= min_accel) {
+                scale_20bit = true;
+            }
+
+            if (accel_y >= max_accel || accel_y <= min_accel) {
+                scale_20bit = true;
+            }
+
+            if (accel_z >= max_accel || accel_z <= min_accel) {
+                scale_20bit = true;
+            }
+
+            // shift by 2 (2 least significant bits are always 0)
+            accel.x[accel.samples] = accel_x / 4;
+            accel.y[accel.samples] = accel_y / 4;
+            accel.z[accel.samples] = accel_z / 4;
+            accel.samples++;
+        }
+    }
+
+    if (!scale_20bit) {
+        // On the 686, if highres enabled accel data is always 4096 LSB/g
+        // On the 688, if highres enabled accel data is always 8192 LSB/g
+        if (isICM686) {
+            _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+
+        } else {
+            _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+        }
+
+    } else {
+        // 20 bit data scaled to 16 bit (2^4)
+        for (int i = 0; i < samples; i++) {
+            // 20 bit hires mode
+            // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+            // Accel data is 18 bit ()
+            int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+            int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+            int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+            accel.x[i] = accel_x;
+            accel.y[i] = accel_y;
+            accel.z[i] = accel_z;
+        }
+
+        if (isICM686) {
+            _px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
+
+        } else {
+            _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+        }
+    }
+
+    // correct frame for publication
+    for (int i = 0; i < accel.samples; i++) {
+        // sensor's frame is +x forward, +y left, +z up
+        //  flip y & z to publish right handed with z down (x forward, y right, z down)
+        accel.x[i] = accel.x[i];
+        accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+        accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
+    }
+
+    _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+    if (accel.samples > 0) {
+        _px4_accel.updateFIFO(accel);
+    }
 }
 
-void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
-{
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = 0;
-
-	// 19-bits of gyroscope data sent as 20-bits, LSB bit is 0
-	bool scale_20bit = false;
-
-	// first pass
-	for (int i = 0; i < samples; i++) {
-
-		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
-
-		if (_enable_clock_input) {
-			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
-
-		} else {
-			gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
-		}
-
-		// 20 bit hires mode
-		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
-		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
-		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
-
-		// check if any values are going to exceed int16 limits
-		static constexpr int16_t max_gyro = INT16_MAX;
-		static constexpr int16_t min_gyro = INT16_MIN;
-
-		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
-			scale_20bit = true;
-		}
-
-		// shift by 1 (least significant bit is always 0)
-		gyro.x[gyro.samples] = gyro_x / 2;
-		gyro.y[gyro.samples] = gyro_y / 2;
-		gyro.z[gyro.samples] = gyro_z / 2;
-		gyro.samples++;
-	}
-
-	if (!scale_20bit) {
-		// On the 686, if highres enabled gyro data is always 65.5 LSB/dps
-		// On the 688, if highres enabled gyro data is always 131 LSB/dps
-		if (isICM686) {
-			_px4_gyro.set_scale(math::radians(1.f / 65.5f));
-
-		} else {
-			_px4_gyro.set_scale(math::radians(1.f / 131.f));
-		}
-
-	} else {
-		// 20 bit data scaled to 16 bit (2^4)
-		for (int i = 0; i < samples; i++) {
-			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
-			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
-			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
-		}
-
-		if (isICM686) {
-			_px4_gyro.set_scale(math::radians(2000.f / 16384.f));
-
-		} else {
-			_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
-		}
-
-	}
-
-	// correct frame for publication
-	for (int i = 0; i < gyro.samples; i++) {
-		// sensor's frame is +x forward, +y left, +z up
-		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro.x[i];
-		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
-		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
-	}
-
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
-
-	if (gyro.samples > 0) {
-		_px4_gyro.updateFIFO(gyro);
-	}
+void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples) {
+    sensor_gyro_fifo_s gyro{};
+    gyro.timestamp_sample = timestamp_sample;
+    gyro.samples          = 0;
+
+    // 19-bits of gyroscope data sent as 20-bits, LSB bit is 0
+    bool scale_20bit = false;
+
+    // first pass
+    for (int i = 0; i < samples; i++) {
+        uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+        if (_enable_clock_input) {
+            gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+        } else {
+            gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+        }
+
+        // 20 bit hires mode
+        // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+        int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+        int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+        int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+        // check if any values are going to exceed int16 limits
+        static constexpr int16_t max_gyro = INT16_MAX;
+        static constexpr int16_t min_gyro = INT16_MIN;
+
+        if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+            scale_20bit = true;
+        }
+
+        if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+            scale_20bit = true;
+        }
+
+        if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+            scale_20bit = true;
+        }
+
+        // shift by 1 (least significant bit is always 0)
+        gyro.x[gyro.samples] = gyro_x / 2;
+        gyro.y[gyro.samples] = gyro_y / 2;
+        gyro.z[gyro.samples] = gyro_z / 2;
+        gyro.samples++;
+    }
+
+    if (!scale_20bit) {
+        // On the 686, if highres enabled gyro data is always 65.5 LSB/dps
+        // On the 688, if highres enabled gyro data is always 131 LSB/dps
+        if (isICM686) {
+            _px4_gyro.set_scale(math::radians(1.f / 65.5f));
+
+        } else {
+            _px4_gyro.set_scale(math::radians(1.f / 131.f));
+        }
+
+    } else {
+        // 20 bit data scaled to 16 bit (2^4)
+        for (int i = 0; i < samples; i++) {
+            gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+            gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+            gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+        }
+
+        if (isICM686) {
+            _px4_gyro.set_scale(math::radians(2000.f / 16384.f));
+
+        } else {
+            _px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+        }
+    }
+
+    // correct frame for publication
+    for (int i = 0; i < gyro.samples; i++) {
+        // sensor's frame is +x forward, +y left, +z up
+        //  flip y & z to publish right handed with z down (x forward, y right, z down)
+        gyro.x[i] = gyro.x[i];
+        gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+        gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+    }
+
+    _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+
+    if (gyro.samples > 0) {
+        _px4_gyro.updateFIFO(gyro);
+    }
 }
 
-bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
-{
-	int16_t temperature[FIFO_MAX_SAMPLES];
-	float temperature_sum{0};
-
-	int valid_samples = 0;
-
-	for (int i = 0; i < samples; i++) {
-		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
-
-		// sample invalid if -32768
-		if (t != -32768) {
-			temperature_sum += t;
-			temperature[valid_samples] = t;
-			valid_samples++;
-		}
-	}
-
-	if (valid_samples > 0) {
-		const float temperature_avg = temperature_sum / valid_samples;
-
-		for (int i = 0; i < valid_samples; i++) {
-			// temperature changing wildly is an indication of a transfer error
-			if (fabsf(temperature[i] - temperature_avg) > 1000) {
-				perf_count(_bad_transfer_perf);
-				return false;
-			}
-		}
-
-		// use average temperature reading
-		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
-
-		if (PX4_ISFINITE(TEMP_degC)) {
-			_px4_accel.set_temperature(TEMP_degC);
-			_px4_gyro.set_temperature(TEMP_degC);
-			return true;
-
-		} else {
-			perf_count(_bad_transfer_perf);
-		}
-	}
-
-	return false;
+bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples) {
+    int16_t temperature[FIFO_MAX_SAMPLES];
+    float   temperature_sum{0};
+
+    int valid_samples = 0;
+
+    for (int i = 0; i < samples; i++) {
+        const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
+
+        // sample invalid if -32768
+        if (t != -32768) {
+            temperature_sum            += t;
+            temperature[valid_samples]  = t;
+            valid_samples++;
+        }
+    }
+
+    if (valid_samples > 0) {
+        const float temperature_avg = temperature_sum / valid_samples;
+
+        for (int i = 0; i < valid_samples; i++) {
+            // temperature changing wildly is an indication of a transfer error
+            if (fabsf(temperature[i] - temperature_avg) > 1000) {
+                perf_count(_bad_transfer_perf);
+                return false;
+            }
+        }
+
+        // use average temperature reading
+        const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
+
+        if (PX4_ISFINITE(TEMP_degC)) {
+            _px4_accel.set_temperature(TEMP_degC);
+            _px4_gyro.set_temperature(TEMP_degC);
+            return true;
+
+        } else {
+            perf_count(_bad_transfer_perf);
+        }
+    }
+
+    return false;
 }
diff --git a/apps/peripheral/sensor/imu/icm42688p/ICM42688P.hpp b/apps/peripheral/sensor/imu/icm42688p/ICM42688P.hpp
index a04e74e707..9eafc63282 100644
--- a/apps/peripheral/sensor/imu/icm42688p/ICM42688P.hpp
+++ b/apps/peripheral/sensor/imu/icm42688p/ICM42688P.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file ICM42688P.hpp
@@ -53,171 +30,194 @@
 
 using namespace InvenSense_ICM42688P;
 
-class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
-{
+class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P> {
 public:
-	ICM42688P(const I2CSPIDriverConfig &config);
-	~ICM42688P() override;
+    ICM42688P(const I2CSPIDriverConfig &config);
+    ~ICM42688P() override;
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
-
-	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
-	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
-
-	static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input
-
-	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
-
-	// Transfer data
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
-		uint8_t INT_STATUS{0};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	};
-	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
-
-	struct register_bank0_config_t {
-		Register::BANK_0 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank1_config_t {
-		Register::BANK_1 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	struct register_bank2_config_t {
-		Register::BANK_2 reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Reset();
-
-	bool Configure();
-	void ConfigureSampleRate(int sample_rate);
-	void ConfigureFIFOWatermark(uint8_t samples);
-	void ConfigureCLKIN();
-
-	void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
-	void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); }
-	void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); }
-	void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); }
-
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
-
-	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
-	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
-	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
-	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
-
-	uint16_t FIFOReadCount();
-	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
-	void FIFOReset();
-
-	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
-	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
-
-	const spi_drdy_gpio_t _drdy_gpio;
-
-	PX4Accelerometer _px4_accel;
-	PX4Gyroscope _px4_gyro;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-	perf_counter_t _drdy_missed_perf{nullptr};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	hrt_abstime _temperature_update_timestamp{0};
-	int _failure_count{0};
-
-	bool _enable_clock_input{false};
-	float _input_clock_freq{0.f};
-
-	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0};
-
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _data_ready_interrupt_enabled{false};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		FIFO_RESET,
-		FIFO_READ,
-	} _state{STATE::RESET};
-
-	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
-
-	uint8_t _checked_register_bank0{0};
-	static constexpr uint8_t size_register_bank0_cfg{16};
-	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
-		{ Register::BANK_0::INTF_CONFIG1,         0, 0}, // RTC_MODE[2] set at runtime
-		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
-		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
-		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
-		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
-		{ Register::BANK_0::TMST_CONFIG,          TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN },
-		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN },
-		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
-		{ Register::BANK_0::INT_CONFIG1,          0, INT_CONFIG1_BIT::INT_ASYNC_RESET },
-		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
-	};
-
-	uint8_t _checked_register_bank1{0};
-	static constexpr uint8_t size_register_bank1_cfg{5};
-	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
-		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR},
-		{ Register::BANK_1::INTF_CONFIG5,         0, 0 },
-	};
-
-	uint8_t _checked_register_bank2{0};
-	static constexpr uint8_t size_register_bank2_cfg{3};
-	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
-		// Register                              | Set bits, Clear bits
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR },
-	};
-	bool isICM686{false};
+    void exit_and_cleanup() override;
+
+    // Sensor Configuration
+    static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
+    static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+    static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+    static constexpr float FIFO_TIMESTAMP_SCALING{16.f * (32.f / 30.f)}; // Used when not using clock input
+
+    // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
+    static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+
+    // Transfer data
+    struct FIFOTransferBuffer {
+        uint8_t    cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
+        uint8_t    INT_STATUS{0};
+        uint8_t    FIFO_COUNTH{0};
+        uint8_t    FIFO_COUNTL{0};
+        FIFO::DATA f[FIFO_MAX_SAMPLES]{};
+    };
+
+    // ensure no struct padding
+    static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
+
+    struct register_bank0_config_t {
+        Register::BANK_0 reg;
+        uint8_t          set_bits{0};
+        uint8_t          clear_bits{0};
+    };
+
+    struct register_bank1_config_t {
+        Register::BANK_1 reg;
+        uint8_t          set_bits{0};
+        uint8_t          clear_bits{0};
+    };
+
+    struct register_bank2_config_t {
+        Register::BANK_2 reg;
+        uint8_t          set_bits{0};
+        uint8_t          clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Reset();
+
+    bool Configure();
+    void ConfigureSampleRate(int sample_rate);
+    void ConfigureFIFOWatermark(uint8_t samples);
+    void ConfigureCLKIN();
+
+    void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
+
+    void SelectRegisterBank(Register::BANK_0 reg) {
+        SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+    }
+
+    void SelectRegisterBank(Register::BANK_1 reg) {
+        SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1);
+    }
+
+    void SelectRegisterBank(Register::BANK_2 reg) {
+        SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2);
+    }
+
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
+
+    template <typename T>
+    bool RegisterCheck(const T &reg_cfg);
+    template <typename T>
+    uint8_t RegisterRead(T reg);
+    template <typename T>
+    void RegisterWrite(T reg, uint8_t value);
+    template <typename T>
+    void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
+
+    template <typename T>
+    void RegisterSetBits(T reg, uint8_t setbits) {
+        RegisterSetAndClearBits(reg, setbits, 0);
+    }
+
+    template <typename T>
+    void RegisterClearBits(T reg, uint8_t clearbits) {
+        RegisterSetAndClearBits(reg, 0, clearbits);
+    }
+
+    uint16_t FIFOReadCount();
+    bool     FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+    void     FIFOReset();
+
+    void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+    void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+    bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
+    const spi_drdy_gpio_t _drdy_gpio;
+
+    PX4Accelerometer _px4_accel;
+    PX4Gyroscope     _px4_gyro;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
+    perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME ": FIFO empty")};
+    perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME ": FIFO overflow")};
+    perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": FIFO reset")};
+    perf_counter_t _drdy_missed_perf{nullptr};
+
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_config_check_timestamp{0};
+    hrt_abstime _temperature_update_timestamp{0};
+    int         _failure_count{0};
+
+    bool  _enable_clock_input{false};
+    float _input_clock_freq{0.f};
+
+    enum REG_BANK_SEL_BIT _last_register_bank {
+        REG_BANK_SEL_BIT::BANK_SEL_0
+    };
+
+    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                     _data_ready_interrupt_enabled{false};
+
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        FIFO_RESET,
+        FIFO_READ,
+    } _state{STATE::RESET};
+
+    uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
+    int32_t  _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+
+    uint8_t                  _checked_register_bank0{0};
+    static constexpr uint8_t size_register_bank0_cfg{16};
+    register_bank0_config_t  _register_bank0_cfg[size_register_bank0_cfg]{
+        // Register                              | Set bits, Clear bits
+        {Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY},
+        {Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0},
+        {Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime
+        {Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0},
+        {Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR},
+        {Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR},
+        {Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD},
+        {Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW},
+        {Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD},
+        {Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN},
+        {Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN},
+        {Register::BANK_0::FIFO_CONFIG2, 0, 0}, // FIFO_WM[7:0] set at runtime
+        {Register::BANK_0::FIFO_CONFIG3, 0, 0}, // FIFO_WM[11:8] set at runtime
+        {Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0},
+        {Register::BANK_0::INT_CONFIG1, 0, INT_CONFIG1_BIT::INT_ASYNC_RESET},
+        {Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0},
+    };
+
+    uint8_t                  _checked_register_bank1{0};
+    static constexpr uint8_t size_register_bank1_cfg{5};
+    register_bank1_config_t  _register_bank1_cfg[size_register_bank1_cfg]{
+        // Register                              | Set bits, Clear bits
+        {Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS},
+        {Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR},
+        {Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR},
+        {Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR},
+        {Register::BANK_1::INTF_CONFIG5, 0, 0},
+    };
+
+    uint8_t                  _checked_register_bank2{0};
+    static constexpr uint8_t size_register_bank2_cfg{3};
+    register_bank2_config_t  _register_bank2_cfg[size_register_bank2_cfg]{
+        // Register                              | Set bits, Clear bits
+        {Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS},
+        {Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR},
+        {Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR},
+    };
+    bool isICM686{false};
 };
diff --git a/apps/peripheral/sensor/imu/icm42688p/InvenSense_ICM42688P_registers.hpp b/apps/peripheral/sensor/imu/icm42688p/InvenSense_ICM42688P_registers.hpp
index 6c407216f0..83afa87fea 100644
--- a/apps/peripheral/sensor/imu/icm42688p/InvenSense_ICM42688P_registers.hpp
+++ b/apps/peripheral/sensor/imu/icm42688p/InvenSense_ICM42688P_registers.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file InvenSense_ICM42688P_registers.hpp
@@ -43,8 +20,7 @@
 #include <cstdint>
 #include <cstddef>
 
-namespace InvenSense_ICM42688P
-{
+namespace InvenSense_ICM42688P {
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -56,363 +32,360 @@ static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
 static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
-static constexpr uint8_t DIR_READ = 0x80;
+static constexpr uint8_t  DIR_READ  = 0x80;
 
-static constexpr uint8_t WHOAMI = 0x47;
+static constexpr uint8_t WHOAMI    = 0x47;
 static constexpr uint8_t WHOAMI686 = 0x44;
 
 static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
-static constexpr float TEMPERATURE_OFFSET = 25.f; // C
+static constexpr float TEMPERATURE_OFFSET      = 25.f;    // C
 
-namespace Register
-{
+namespace Register {
 
 enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG      = 0x11,
+    DEVICE_CONFIG = 0x11,
 
-	INT_CONFIG         = 0x14,
+    INT_CONFIG = 0x14,
 
-	FIFO_CONFIG        = 0x16,
+    FIFO_CONFIG = 0x16,
 
-	TEMP_DATA1         = 0x1D,
-	TEMP_DATA0         = 0x1E,
+    TEMP_DATA1 = 0x1D,
+    TEMP_DATA0 = 0x1E,
 
-	INT_STATUS         = 0x2D,
-	FIFO_COUNTH        = 0x2E,
-	FIFO_COUNTL        = 0x2F,
-	FIFO_DATA          = 0x30,
+    INT_STATUS  = 0x2D,
+    FIFO_COUNTH = 0x2E,
+    FIFO_COUNTL = 0x2F,
+    FIFO_DATA   = 0x30,
 
-	SIGNAL_PATH_RESET  = 0x4B,
-	INTF_CONFIG0       = 0x4C,
-	INTF_CONFIG1       = 0x4D,
-	PWR_MGMT0          = 0x4E,
-	GYRO_CONFIG0       = 0x4F,
-	ACCEL_CONFIG0      = 0x50,
-	GYRO_CONFIG1       = 0x51,
-	GYRO_ACCEL_CONFIG0 = 0x52,
-	ACCEL_CONFIG1      = 0x53,
-	TMST_CONFIG        = 0x54,
+    SIGNAL_PATH_RESET  = 0x4B,
+    INTF_CONFIG0       = 0x4C,
+    INTF_CONFIG1       = 0x4D,
+    PWR_MGMT0          = 0x4E,
+    GYRO_CONFIG0       = 0x4F,
+    ACCEL_CONFIG0      = 0x50,
+    GYRO_CONFIG1       = 0x51,
+    GYRO_ACCEL_CONFIG0 = 0x52,
+    ACCEL_CONFIG1      = 0x53,
+    TMST_CONFIG        = 0x54,
 
-	FIFO_CONFIG1       = 0x5F,
-	FIFO_CONFIG2       = 0x60,
-	FIFO_CONFIG3       = 0x61,
+    FIFO_CONFIG1 = 0x5F,
+    FIFO_CONFIG2 = 0x60,
+    FIFO_CONFIG3 = 0x61,
 
-	INT_CONFIG0        = 0x63,
-	INT_CONFIG1        = 0x64,
+    INT_CONFIG0 = 0x63,
+    INT_CONFIG1 = 0x64,
 
-	INT_SOURCE0        = 0x65,
+    INT_SOURCE0 = 0x65,
 
-	SELF_TEST_CONFIG   = 0x70,
+    SELF_TEST_CONFIG = 0x70,
 
-	WHO_AM_I           = 0x75,
-	REG_BANK_SEL       = 0x76,
+    WHO_AM_I     = 0x75,
+    REG_BANK_SEL = 0x76,
 };
 
 enum class BANK_1 : uint8_t {
-	GYRO_CONFIG_STATIC2  = 0x0B,
-	GYRO_CONFIG_STATIC3  = 0x0C,
-	GYRO_CONFIG_STATIC4  = 0x0D,
-	GYRO_CONFIG_STATIC5  = 0x0E,
+    GYRO_CONFIG_STATIC2 = 0x0B,
+    GYRO_CONFIG_STATIC3 = 0x0C,
+    GYRO_CONFIG_STATIC4 = 0x0D,
+    GYRO_CONFIG_STATIC5 = 0x0E,
 
-	INTF_CONFIG5         = 0x7B,
+    INTF_CONFIG5 = 0x7B,
 };
 
 enum class BANK_2 : uint8_t {
-	ACCEL_CONFIG_STATIC2 = 0x03,
-	ACCEL_CONFIG_STATIC3 = 0x04,
-	ACCEL_CONFIG_STATIC4 = 0x05,
+    ACCEL_CONFIG_STATIC2 = 0x03,
+    ACCEL_CONFIG_STATIC3 = 0x04,
+    ACCEL_CONFIG_STATIC4 = 0x05,
 };
 
-};
+}; // namespace Register
 
 //---------------- BANK0 Register bits
 
 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0, //
+    SOFT_RESET_CONFIG = Bit0, //
 };
 
 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
+    INT1_MODE          = Bit2,
+    INT1_DRIVE_CIRCUIT = Bit1,
+    INT1_POLARITY      = Bit0,
 };
 
 // FIFO_CONFIG
 enum FIFO_CONFIG_BIT : uint8_t {
-	// 7:6 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+    // 7:6 FIFO_MODE
+    FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
 };
 
 // INT_STATUS
 enum INT_STATUS_BIT : uint8_t {
-	RESET_DONE_INT = Bit4,
-	DATA_RDY_INT   = Bit3,
-	FIFO_THS_INT   = Bit2,
-	FIFO_FULL_INT  = Bit1,
+    RESET_DONE_INT = Bit4,
+    DATA_RDY_INT   = Bit3,
+    FIFO_THS_INT   = Bit2,
+    FIFO_FULL_INT  = Bit1,
 };
 
 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	ABORT_AND_RESET = Bit3,
-	FIFO_FLUSH      = Bit1,
+    ABORT_AND_RESET = Bit3,
+    FIFO_FLUSH      = Bit1,
 };
 
 enum INTF_CONFIG1_BIT : uint8_t {
-	RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
-	CLKSEL = Bit0,
-	CLKSEL_CLEAR = Bit1,
+    RTC_MODE     = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
+    CLKSEL       = Bit0,
+    CLKSEL_CLEAR = Bit1,
 };
 
 // PWR_MGMT0
 enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+    GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+    ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
 };
 
 // GYRO_CONFIG0
 enum GYRO_CONFIG0_BIT : uint8_t {
-	// 7:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
-	GYRO_FS_SEL_1000_DPS = Bit5,
-	GYRO_FS_SEL_500_DPS  = Bit6,
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
-	GYRO_FS_SEL_125_DPS  = Bit7,
-
-
-	// 3:0 GYRO_ODR
-	//  0001: 32kHz
-	GYRO_ODR_32KHZ_SET   = Bit0,
-	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	GYRO_ODR_16KHZ_SET   = Bit1,
-	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
-	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
-	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+    // 7:5 GYRO_FS_SEL
+    GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
+    GYRO_FS_SEL_1000_DPS = Bit5,
+    GYRO_FS_SEL_500_DPS  = Bit6,
+    GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
+    GYRO_FS_SEL_125_DPS  = Bit7,
+
+
+    // 3:0 GYRO_ODR
+    //  0001: 32kHz
+    GYRO_ODR_32KHZ_SET   = Bit0,
+    GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+    //  0010: 16kHz
+    GYRO_ODR_16KHZ_SET   = Bit1,
+    GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+    //  0011: 8kHz
+    GYRO_ODR_8KHZ_SET   = Bit1 | Bit0,
+    GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
+    //  0110: 1kHz (default)
+    GYRO_ODR_1KHZ_SET   = Bit2 | Bit1,
+    GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
 };
 
 // ACCEL_CONFIG0
 enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:5 ACCEL_FS_SEL
-	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
-	ACCEL_FS_SEL_8G  = Bit5,
-	ACCEL_FS_SEL_4G  = Bit6,
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
-
-
-	// 3:0 ACCEL_ODR
-	//  0001: 32kHz
-	ACCEL_ODR_32KHZ_SET   = Bit0,
-	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0010: 16kHz
-	ACCEL_ODR_16KHZ_SET   = Bit1,
-	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
-	//  0011: 8kHz
-	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
-	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
-	//  0110: 1kHz (default)
-	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
-	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
+    // 7:5 ACCEL_FS_SEL
+    ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
+    ACCEL_FS_SEL_8G  = Bit5,
+    ACCEL_FS_SEL_4G  = Bit6,
+    ACCEL_FS_SEL_2G  = Bit6 | Bit5,
+
+
+    // 3:0 ACCEL_ODR
+    //  0001: 32kHz
+    ACCEL_ODR_32KHZ_SET   = Bit0,
+    ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+    //  0010: 16kHz
+    ACCEL_ODR_16KHZ_SET   = Bit1,
+    ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+    //  0011: 8kHz
+    ACCEL_ODR_8KHZ_SET   = Bit1 | Bit0,
+    ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
+    //  0110: 1kHz (default)
+    ACCEL_ODR_1KHZ_SET   = Bit2 | Bit1,
+    ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
 };
 
 // GYRO_CONFIG1
 enum GYRO_CONFIG1_BIT : uint8_t {
-	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
+    GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
 };
 
 // GYRO_ACCEL_CONFIG0
 enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
-	// 7:4 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+    // 7:4 ACCEL_UI_FILT_BW
+    ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
 
-	// 3:0 GYRO_UI_FILT_BW
-	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+    // 3:0 GYRO_UI_FILT_BW
+    GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
 };
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
+    ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
 };
 
 // TMST_CONFIG
 enum TMST_CONFIG_BIT : uint8_t {
-	TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value
-	TMST_RES	= Bit3, // 0: 1us resolution, 1: 16us resolution or 1 RTC period when clock is used
-	TMST_DELTA_EN   = Bit2, // 1: Time Stamp delta enable
-	TMST_FSYNC_EN   = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled
-	TMST_EN		= Bit0, // 1: Time Stamp register enable (default)
+    TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value
+    TMST_RES        = Bit3, // 0: 1us resolution, 1: 16us resolution or 1 RTC period when clock is used
+    TMST_DELTA_EN   = Bit2, // 1: Time Stamp delta enable
+    TMST_FSYNC_EN   = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled
+    TMST_EN         = Bit0, // 1: Time Stamp register enable (default)
 };
 
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	FIFO_RESUME_PARTIAL_RD = Bit6,
-	FIFO_WM_GT_TH          = Bit5,
-	FIFO_HIRES_EN          = Bit4,
-	FIFO_TMST_FSYNC_EN     = Bit3,
-	FIFO_TEMP_EN           = Bit2,
-	FIFO_GYRO_EN           = Bit1,
-	FIFO_ACCEL_EN          = Bit0,
+    FIFO_RESUME_PARTIAL_RD = Bit6,
+    FIFO_WM_GT_TH          = Bit5,
+    FIFO_HIRES_EN          = Bit4,
+    FIFO_TMST_FSYNC_EN     = Bit3,
+    FIFO_TEMP_EN           = Bit2,
+    FIFO_GYRO_EN           = Bit1,
+    FIFO_ACCEL_EN          = Bit0,
 };
 
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
-	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
+    // 3:2 FIFO_THS_INT_CLEAR
+    CLEAR_ON_FIFO_READ = Bit3,
 };
 
 // INT_CONFIG1
 enum INT_CONFIG1_BIT : uint8_t {
-	INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
+    INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
 };
 
 // INT_SOURCE0
 enum INT_SOURCE0_BIT : uint8_t {
-	UI_FSYNC_INT1_EN   = Bit6,
-	PLL_RDY_INT1_EN    = Bit5,
-	RESET_DONE_INT1_EN = Bit4,
-	UI_DRDY_INT1_EN    = Bit3,
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
-	FIFO_FULL_INT1_EN  = Bit1,
-	UI_AGC_RDY_INT1_EN = Bit0,
+    UI_FSYNC_INT1_EN   = Bit6,
+    PLL_RDY_INT1_EN    = Bit5,
+    RESET_DONE_INT1_EN = Bit4,
+    UI_DRDY_INT1_EN    = Bit3,
+    FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+    FIFO_FULL_INT1_EN  = Bit1,
+    UI_AGC_RDY_INT1_EN = Bit0,
 };
 
 // REG_BANK_SEL
 enum REG_BANK_SEL_BIT : uint8_t {
-	// 2:0 BANK_SEL
-	BANK_SEL_0 = 0,           // 000: Bank 0 (default)
-	BANK_SEL_1 = Bit0,        // 001: Bank 1
-	BANK_SEL_2 = Bit1,        // 010: Bank 2
-	BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3
-	BANK_SEL_4 = Bit2,        // 100: Bank 4
+    // 2:0 BANK_SEL
+    BANK_SEL_0 = 0,           // 000: Bank 0 (default)
+    BANK_SEL_1 = Bit0,        // 001: Bank 1
+    BANK_SEL_2 = Bit1,        // 010: Bank 2
+    BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3
+    BANK_SEL_4 = Bit2,        // 100: Bank 4
 };
 
-
 //---------------- BANK1 Register bits
 
 // GYRO_CONFIG_STATIC2
 enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
-	GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter
-	GYRO_NF_DIS  = Bit0, // 1: Disable Notch Filter
+    GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter
+    GYRO_NF_DIS  = Bit0, // 1: Disable Notch Filter
 };
 
 // GYRO_CONFIG_STATIC3
 enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
-	// 5:0 GYRO_AAF_DELT
-	//  585 Hz = 13 (0b00'1101)
-	GYRO_AAF_DELT_585HZ_SET   = Bit3 | Bit2 | Bit0,
-	GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1,
+    // 5:0 GYRO_AAF_DELT
+    //  585 Hz = 13 (0b00'1101)
+    GYRO_AAF_DELT_585HZ_SET   = Bit3 | Bit2 | Bit0,
+    GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1,
 };
 
 // GYRO_CONFIG_STATIC4
 enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
-	// 7:0 GYRO_AAF_DELTSQR
-	//  585 Hz = 170 (0b1010'1010)
-	GYRO_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
-	GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+    // 7:0 GYRO_AAF_DELTSQR
+    //  585 Hz = 170 (0b1010'1010)
+    GYRO_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
+    GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
 };
 
 // GYRO_CONFIG_STATIC5
 enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
-	// 7:4 GYRO_AAF_BITSHIFT
-	//  585 Hz = 8 (0b1000)
-	GYRO_AAF_BITSHIFT_585HZ_SET      = Bit7,
-	GYRO_AAF_BITSHIFT_585HZ_CLEAR    = Bit6 | Bit5 | Bit4,
-
-	// 3:0 GYRO_AAF_DELTSQR[11:8]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	GYRO_AAF_DELTSQR_MSB_585HZ_SET   = 0,
-	GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
+    // 7:4 GYRO_AAF_BITSHIFT
+    //  585 Hz = 8 (0b1000)
+    GYRO_AAF_BITSHIFT_585HZ_SET   = Bit7,
+    GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
+
+    // 3:0 GYRO_AAF_DELTSQR[11:8]
+    //  585 Hz = 170 (0b0000'1010'1010)
+    GYRO_AAF_DELTSQR_MSB_585HZ_SET   = 0,
+    GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
 };
 
 // INTF_CONFIG5
 enum INTF_CONFIG5_BIT : uint8_t {
-	// 2:1 PIN9_FUNCTION
-	PIN9_FUNCTION_CLKIN_SET   = Bit2, // 0b10: CLKIN
-	PIN9_FUNCTION_CLKIN_CLEAR = Bit1,
+    // 2:1 PIN9_FUNCTION
+    PIN9_FUNCTION_CLKIN_SET   = Bit2, // 0b10: CLKIN
+    PIN9_FUNCTION_CLKIN_CLEAR = Bit1,
 
-	PIN9_FUNCTION_RESET_SET   = 0,
-	PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1,
+    PIN9_FUNCTION_RESET_SET   = 0,
+    PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1,
 };
 
 //---------------- BANK2 Register bits
 
 // ACCEL_CONFIG_STATIC2
 enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
-	// 6:1 ACCEL_AAF_DELT
-	//  585 Hz = 13 (0b00'1101)
-	ACCEL_AAF_DELT_585HZ_SET   = Bit4 | Bit3 | Bit1,
-	ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2,
+    // 6:1 ACCEL_AAF_DELT
+    //  585 Hz = 13 (0b00'1101)
+    ACCEL_AAF_DELT_585HZ_SET   = Bit4 | Bit3 | Bit1,
+    ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2,
 
-	// 0 ACCEL_AAF_DIS
-	ACCEL_AAF_DIS              = Bit0, // 0: Enable accelerometer anti-aliasing filter (default)
+    // 0 ACCEL_AAF_DIS
+    ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default)
 };
 
 // ACCEL_CONFIG_STATIC3
 enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
-	// 7:0 ACCEL_AAF_DELTSQR[7:0]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	ACCEL_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
-	ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
+    // 7:0 ACCEL_AAF_DELTSQR[7:0]
+    //  585 Hz = 170 (0b0000'1010'1010)
+    ACCEL_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
+    ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
 };
 
 // ACCEL_CONFIG_STATIC4
 enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
-	// 7:4 ACCEL_AAF_BITSHIFT
-	//  585 Hz = 8 (0b1000)
-	ACCEL_AAF_BITSHIFT_585HZ_SET   = Bit7,
-	ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
-
-	// 3:0 ACCEL_AAF_DELTSQR[11:8]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	ACCEL_AAF_DELTSQR_MSB_SET      = 0,
-	ACCEL_AAF_DELTSQR_MSB_CLEAR    = Bit3 | Bit2 | Bit1 | Bit0,
+    // 7:4 ACCEL_AAF_BITSHIFT
+    //  585 Hz = 8 (0b1000)
+    ACCEL_AAF_BITSHIFT_585HZ_SET   = Bit7,
+    ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
+
+    // 3:0 ACCEL_AAF_DELTSQR[11:8]
+    //  585 Hz = 170 (0b0000'1010'1010)
+    ACCEL_AAF_DELTSQR_MSB_SET   = 0,
+    ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
 };
 
-namespace FIFO
-{
+namespace FIFO {
 static constexpr size_t SIZE = 2048;
 
 // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
 
 // Packet 4
 struct DATA {
-	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
-	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
-	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
-	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
-	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
-	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
-	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
-	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
-	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
-	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
-	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
-	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
-	uint8_t TEMP_DATA1;    // Temperature[15:8]
-	uint8_t TEMP_DATA0;    // Temperature[7:0]
-	uint8_t TimeStamp_h;   // TimeStamp[15:8]
-	uint8_t TimeStamp_l;   // TimeStamp[7:0]
-	uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
-	uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
-	uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
+    uint8_t FIFO_Header;
+    uint8_t ACCEL_DATA_X1;      // Accel X [19:12]
+    uint8_t ACCEL_DATA_X0;      // Accel X [11:4]
+    uint8_t ACCEL_DATA_Y1;      // Accel Y [19:12]
+    uint8_t ACCEL_DATA_Y0;      // Accel Y [11:4]
+    uint8_t ACCEL_DATA_Z1;      // Accel Z [19:12]
+    uint8_t ACCEL_DATA_Z0;      // Accel Z [11:4]
+    uint8_t GYRO_DATA_X1;       // Gyro X [19:12]
+    uint8_t GYRO_DATA_X0;       // Gyro X [11:4]
+    uint8_t GYRO_DATA_Y1;       // Gyro Y [19:12]
+    uint8_t GYRO_DATA_Y0;       // Gyro Y [11:4]
+    uint8_t GYRO_DATA_Z1;       // Gyro Z [19:12]
+    uint8_t GYRO_DATA_Z0;       // Gyro Z [11:4]
+    uint8_t TEMP_DATA1;         // Temperature[15:8]
+    uint8_t TEMP_DATA0;         // Temperature[7:0]
+    uint8_t TimeStamp_h;        // TimeStamp[15:8]
+    uint8_t TimeStamp_l;        // TimeStamp[7:0]
+    uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
+    uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
+    uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
 enum FIFO_HEADER_BIT : uint8_t {
-	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
-	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
-	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
-	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
-	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
-	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+    HEADER_MSG             = Bit7,        // 1: FIFO is empty
+    HEADER_ACCEL           = Bit6,        // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+    HEADER_GYRO            = Bit5,        // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+    HEADER_20              = Bit4,        // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+    HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
+    HEADER_ODR_ACCEL       = Bit1,        // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+    HEADER_ODR_GYRO        = Bit0,        // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
 };
 
-}
+} // namespace FIFO
 } // namespace InvenSense_ICM42688P
diff --git a/apps/peripheral/sensor/imu/icm42688p/icm42688p_main.cpp b/apps/peripheral/sensor/imu/icm42688p/icm42688p_main.cpp
index 0794f5cdef..c834610b1c 100644
--- a/apps/peripheral/sensor/imu/icm42688p/icm42688p_main.cpp
+++ b/apps/peripheral/sensor/imu/icm42688p/icm42688p_main.cpp
@@ -1,98 +1,73 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "ICM42688P.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void ICM42688P::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
-	PRINT_MODULE_USAGE_PARAM_FLAG('6', "Drive ICM-42686", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void ICM42688P::print_usage() {
+    PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
+    PRINT_MODULE_USAGE_PARAM_FLAG('6', "Drive ICM-42686", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int icm42688p_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = ICM42688P;
-	BusCLIArguments cli{false, true};
-	cli.default_spi_frequency = SPI_SPEED;
+extern "C" int icm42688p_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = ICM42688P;
+    BusCLIArguments cli{false, true};
+    cli.default_spi_frequency = SPI_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "C:R:6")) != EOF) {
-		switch (ch) {
-		case 'C':
-			cli.custom1 = atoi(cli.optArg());
-			break;
+    while ((ch = cli.getOpt(argc, argv, "C:R:6")) != EOF) {
+        switch (ch) {
+        case 'C':
+            cli.custom1 = atoi(cli.optArg());
+            break;
 
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
 
-		case '6':
-			cli.custom2 = DRV_IMU_DEVTYPE_ICM42686P;
-			break;
-		}
-	}
+        case '6':
+            cli.custom2 = DRV_IMU_DEVTYPE_ICM42686P;
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? "icm42686p" : MODULE_NAME, cli,
-				     cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? DRV_IMU_DEVTYPE_ICM42686P : DRV_IMU_DEVTYPE_ICM42688P);
+    BusInstanceIterator iterator(cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? "icm42686p" : MODULE_NAME, cli,
+                                 cli.custom2 == DRV_IMU_DEVTYPE_ICM42686P ? DRV_IMU_DEVTYPE_ICM42686P : DRV_IMU_DEVTYPE_ICM42688P);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/mag/bmm150/BMM150.cpp b/apps/peripheral/sensor/mag/bmm150/BMM150.cpp
index d761ae513f..0435f5edad 100644
--- a/apps/peripheral/sensor/mag/bmm150/BMM150.cpp
+++ b/apps/peripheral/sensor/mag/bmm150/BMM150.cpp
@@ -1,519 +1,475 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMM150.hpp"
 
 using namespace time_literals;
 
 BMM150::BMM150(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config),
-	_px4_mag(get_device_id(), config.rotation)
-{
+    I2C(config),
+    I2CSPIDriver(config),
+    _px4_mag(get_device_id(), config.rotation) {
 }
 
-BMM150::~BMM150()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_reset_perf);
-	perf_free(_overflow_perf);
-	perf_free(_self_test_failed_perf);
+BMM150::~BMM150() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_reset_perf);
+    perf_free(_overflow_perf);
+    perf_free(_self_test_failed_perf);
 }
 
-int BMM150::init()
-{
-	int ret = I2C::init();
+int BMM150::init() {
+    int ret = I2C::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("I2C::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("I2C::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool BMM150::Reset()
-{
-	RegisterWrite(Register::POWER_CONTROL, 0);
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleDelayed(1_ms);
-	return true;
+bool BMM150::Reset() {
+    RegisterWrite(Register::POWER_CONTROL, 0);
+    _state = STATE::RESET;
+    ScheduleClear();
+    ScheduleDelayed(1_ms);
+    return true;
 }
 
-void BMM150::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void BMM150::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_overflow_perf);
-	perf_print_counter(_self_test_failed_perf);
+    perf_print_counter(_reset_perf);
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_overflow_perf);
+    perf_print_counter(_self_test_failed_perf);
 }
 
-int BMM150::probe()
-{
-	// 3 retries
-	for (int i = 0; i < 3; i++) {
-		const uint8_t POWER_CONTROL = RegisterRead(Register::POWER_CONTROL);
-		const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID);
+int BMM150::probe() {
+    // 3 retries
+    for (int i = 0; i < 3; i++) {
+        const uint8_t POWER_CONTROL = RegisterRead(Register::POWER_CONTROL);
+        const uint8_t CHIP_ID       = RegisterRead(Register::CHIP_ID);
 
-		PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID);
+        PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID);
 
-		if (CHIP_ID == chip_identification_number) {
-			return PX4_OK;
+        if (CHIP_ID == chip_identification_number) {
+            return PX4_OK;
 
-		} else if ((CHIP_ID == 0) && !(POWER_CONTROL & POWER_CONTROL_BIT::PowerControl)) {
-			// in suspend Chip ID read (register 0x40) returns “0x00” (I²C) or high-Z (SPI).
-			return PX4_OK;
-		}
-	}
+        } else if ((CHIP_ID == 0) && !(POWER_CONTROL & POWER_CONTROL_BIT::PowerControl)) {
+            // in suspend Chip ID read (register 0x40) returns “0x00” (I²C) or high-Z (SPI).
+            return PX4_OK;
+        }
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall)
-{
-	float retval = 0;
-
-	// Overflow condition check
-	if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
-		// Processing compensation equations
-		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
-		float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall);
-		retval = (process_comp_x0 - 16384.0f);
-		float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
-		float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
-		float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f;
-		float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3);
-		retval = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f;
-	}
-
-	return retval;
+float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall) {
+    float retval = 0;
+
+    // Overflow condition check
+    if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+        // Processing compensation equations
+        //  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25
+        float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall);
+        retval                = (process_comp_x0 - 16384.0f);
+        float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+        float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+        float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f;
+        float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3);
+        retval                = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f;
+    }
+
+    return retval;
 }
 
-float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall)
-{
-	float retval = 0;
-
-	// Overflow condition check
-	if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
-		// Processing compensation equations
-		float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall;
-		retval = process_comp_y0 - 16384.0f;
-		float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
-		float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
-		float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
-		float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3);
-		retval = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f;
-	}
-
-	return retval;
+float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall) {
+    float retval = 0;
+
+    // Overflow condition check
+    if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) {
+        // Processing compensation equations
+        float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall;
+        retval                = process_comp_y0 - 16384.0f;
+        float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f);
+        float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f;
+        float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f;
+        float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3);
+        retval                = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f;
+    }
+
+    return retval;
 }
 
-float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall)
-{
-	float retval = 0;
-
-	// Overflow condition check
-	if ((mag_data_z != OVERFLOW_ZAXIS)
-	    && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0)
-	    && (data_rhall != 0)) {
-		// Processing compensation equations
-		//  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
-		float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
-		float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
-		float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
-		float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f;
-		float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
-		float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
-		retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f;
-	}
-
-	return retval;
+float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall) {
+    float retval = 0;
+
+    // Overflow condition check
+    if ((mag_data_z != OVERFLOW_ZAXIS)
+        && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0)
+        && (data_rhall != 0)) {
+        // Processing compensation equations
+        //  not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25
+        float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4);
+        float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1);
+        float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1);
+        float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f;
+        float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3;
+        float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2;
+        retval                = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f;
+    }
+
+    return retval;
 }
 
-static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb)
-{
-	// msb: 8-bit MSB part [12:5] of the 13 bit output data
-	// lsb: 5-bit LSB part [4:0] of the 13 bit output data
-	int16_t msb_data = ((int16_t)((int8_t)msb)) << 5;
-	int16_t lsb_data = ((lsb & 0xF8) >> 3);
+static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb) {
+    // msb: 8-bit MSB part [12:5] of the 13 bit output data
+    // lsb: 5-bit LSB part [4:0] of the 13 bit output data
+    int16_t msb_data = ((int16_t)((int8_t)msb)) << 5;
+    int16_t lsb_data = ((lsb & 0xF8) >> 3);
 
-	return (int16_t)(msb_data | lsb_data);
+    return (int16_t)(msb_data | lsb_data);
 }
 
-static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb)
-{
-	// msb: 8-bit MSB part [12:5] of the 13 bit output data
-	// lsb: 7-bit LSB part [6:0] of the 15 bit output data
-	int16_t msb_data = ((int16_t)((int8_t)msb)) << 7;
-	int16_t lsb_data = ((lsb & 0xFE) >> 1);
+static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb) {
+    // msb: 8-bit MSB part [12:5] of the 13 bit output data
+    // lsb: 7-bit LSB part [6:0] of the 15 bit output data
+    int16_t msb_data = ((int16_t)((int8_t)msb)) << 7;
+    int16_t lsb_data = ((lsb & 0xFE) >> 1);
 
-	return (int16_t)(msb_data | lsb_data);
+    return (int16_t)(msb_data | lsb_data);
 }
 
-static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb)
-{
-	// msb: 8-bit MSB part [13:6] of the 14 bit output data
-	// lsb: 6-bit LSB part [5:0] of the 14 bit output data
-	uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6;
-	uint16_t lsb_data = ((lsb & 0xFC) >> 2);
+static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb) {
+    // msb: 8-bit MSB part [13:6] of the 14 bit output data
+    // lsb: 6-bit LSB part [5:0] of the 14 bit output data
+    uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6;
+    uint16_t lsb_data = ((lsb & 0xFC) >> 2);
 
-	return (uint16_t)(msb_data | lsb_data);
+    return (uint16_t)(msb_data | lsb_data);
 }
 
-void BMM150::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// POWER_CONTROL: soft reset
-		RegisterWrite(Register::POWER_CONTROL, POWER_CONTROL_BIT::SoftReset | POWER_CONTROL_BIT::PowerControl);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		perf_count(_reset_perf);
-		ScheduleDelayed(10_ms); // 3.0 ms start-up time from suspend to sleep
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-
-		// Soft reset always brings the device into sleep mode (power off -> suspend -> sleep)
-		if ((RegisterRead(Register::CHIP_ID) == chip_identification_number)
-		    && (RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::PowerControl)
-		    && !(RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::SoftReset)
-		    && (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) {
-
-			// if reset succeeded then start self test
-			RegisterSetBits(Register::OP_MODE, OP_MODE_BIT::Self_Test);
-
-			_state = STATE::SELF_TEST_CHECK;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::SELF_TEST_CHECK: {
-			// After performing self test OpMode "Self test" bit is set to 0
-			const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0);
-
-			if (opmode_self_test_cleared) {
-				// When self-test is successful, the corresponding self-test result bits are set
-				//  “X-Self-Test” register 0x42 bit0
-				//  “Y-Self-Test” register 0x44 bit0
-				//  “Z-Self-Test” register 0x46 bit0
-				const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0;
-				const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0;
-				const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0;
-
-				if (x_success && y_success && z_success) {
-					_state = STATE::READ_TRIM;
-					ScheduleDelayed(10_ms);
-
-				} else {
-					if (perf_event_count(_self_test_failed_perf) >= 5) {
-						PX4_ERR("self test still failing after 5 attempts");
-
-						// reluctantly proceed
-						_state = STATE::READ_TRIM;
-						ScheduleDelayed(10_ms);
-
-					} else {
-						PX4_ERR("self test failed, resetting");
-						perf_count(_self_test_failed_perf);
-						_state = STATE::RESET;
-						ScheduleDelayed(1_s);
-					}
-				}
-
-			} else {
-				if (hrt_elapsed_time(&_reset_timestamp) < 3_s) {
-					// self test not complete, check again in 100 milliseconds
-					_state = STATE::SELF_TEST_CHECK;
-					ScheduleDelayed(100_ms);
-
-				} else {
-					// full reset
-					_state = STATE::RESET;
-					ScheduleDelayed(10_ms);
-				}
-			}
-		}
-
-		break;
-
-	case STATE::READ_TRIM: {
-			// Trim register value is read
-			uint8_t cmd = static_cast<uint8_t>(Register::DIG_X1);
-			uint8_t trim_x1y1[2] {};
-
-			if (transfer(&cmd, 1, trim_x1y1, 2) == PX4_OK) {
-				cmd = static_cast<uint8_t>(Register::DIG_Z4_LSB);
-				uint8_t trim_xyz_data[4] {};
-
-				if (transfer(&cmd, 1, trim_xyz_data, 4) == PX4_OK) {
-					cmd = static_cast<uint8_t>(Register::DIG_Z2_LSB);
-					uint8_t trim_xy1xy2[10] {};
-
-					if (transfer(&cmd, 1, trim_xy1xy2, 10) == PX4_OK) {
-						_trim_data.dig_x1 = (int8_t)trim_x1y1[0];
-						_trim_data.dig_y1 = (int8_t)trim_x1y1[1];
-
-						_trim_data.dig_x2 = (int8_t)trim_xyz_data[2];
-						_trim_data.dig_y2 = (int8_t)trim_xyz_data[3];
-
-						uint16_t temp_msb;
-						temp_msb = ((uint16_t)trim_xy1xy2[3]) << 8;
-						_trim_data.dig_z1 = (uint16_t)(temp_msb | trim_xy1xy2[2]);
-
-						temp_msb = ((uint16_t)trim_xy1xy2[1]) << 8;
-						_trim_data.dig_z2 = (int16_t)(temp_msb | trim_xy1xy2[0]);
-
-						temp_msb = ((uint16_t)trim_xy1xy2[7]) << 8;
-						_trim_data.dig_z3 = (int16_t)(temp_msb | trim_xy1xy2[6]);
-
-						temp_msb = ((uint16_t)trim_xyz_data[1]) << 8;
-						_trim_data.dig_z4 = (int16_t)(temp_msb | trim_xyz_data[0]);
-
-						_trim_data.dig_xy1 = trim_xy1xy2[9];
-						_trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8];
-
-						temp_msb = ((uint16_t)(trim_xy1xy2[5] & 0x7F)) << 8;
-						_trim_data.dig_xyz1 = (uint16_t)(temp_msb | trim_xy1xy2[4]);
-
-						if ((_trim_data.dig_xyz1 != 0) && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0)) {
-							_state = STATE::CONFIGURE;
-							ScheduleDelayed(1_ms);
-							return;
-						}
-					}
-				}
-			}
-
-			// reset if reading trim failed
-			PX4_DEBUG("reading trim failed, resetting");
-			perf_count(_bad_register_perf);
-			_state = STATE::RESET;
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start reading every 50 ms (20 Hz)
-			_state = STATE::READ;
-			ScheduleOnInterval(50_ms, 50_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::READ: {
-			struct TransferBuffer {
-				uint8_t DATAX_LSB;
-				uint8_t DATAX_MSB;
-				uint8_t DATAY_LSB;
-				uint8_t DATAY_MSB;
-				uint8_t DATAZ_LSB;
-				uint8_t DATAZ_MSB;
-				uint8_t RHALL_LSB;
-				uint8_t RHALL_MSB;
-				uint8_t STATUS;
-			} buffer{};
-
-			bool success = false;
-			// 0x42 to 0x4A with a burst read.
-			uint8_t cmd = static_cast<uint8_t>(Register::DATAX_LSB);
-
-			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
-
-				int16_t x = combine_xy_int13(buffer.DATAX_MSB, buffer.DATAX_LSB);
-				int16_t y = combine_xy_int13(buffer.DATAY_MSB, buffer.DATAY_LSB);
-				int16_t z = combine_z_int15(buffer.DATAZ_MSB, buffer.DATAZ_LSB);
-				uint16_t rhall = combine_rhall_uint14(buffer.RHALL_MSB, buffer.RHALL_LSB);
-
-				const bool data_ready = buffer.RHALL_LSB & Bit0;
-
-				if (data_ready && (rhall != 0)) {
-					if ((buffer.STATUS & STATUS_BIT::Overflow) ||
-					    (x == OVERFLOW_XYAXES) || (y == OVERFLOW_XYAXES) || (z == OVERFLOW_ZAXIS)) {
-						// overflow ADC value, record error, but don't publish
-						perf_count(_overflow_perf);
-
-					} else {
-						_px4_mag.set_error_count(perf_event_count(_bad_register_perf)
-									 + perf_event_count(_bad_transfer_perf) + perf_event_count(_self_test_failed_perf));
-						_px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall));
-
-						success = true;
-
-						if (_failure_count > 0) {
-							_failure_count--;
-						}
-					}
-				}
-
-			} else {
-				perf_count(_bad_transfer_perf);
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-				}
-			}
-		}
-
-		break;
-	}
+void BMM150::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // POWER_CONTROL: soft reset
+        RegisterWrite(Register::POWER_CONTROL, POWER_CONTROL_BIT::SoftReset | POWER_CONTROL_BIT::PowerControl);
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        perf_count(_reset_perf);
+        ScheduleDelayed(10_ms); // 3.0 ms start-up time from suspend to sleep
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+
+        // Soft reset always brings the device into sleep mode (power off -> suspend -> sleep)
+        if ((RegisterRead(Register::CHIP_ID) == chip_identification_number)
+            && (RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::PowerControl)
+            && !(RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::SoftReset)
+            && (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) {
+            // if reset succeeded then start self test
+            RegisterSetBits(Register::OP_MODE, OP_MODE_BIT::Self_Test);
+
+            _state = STATE::SELF_TEST_CHECK;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::SELF_TEST_CHECK: {
+        // After performing self test OpMode "Self test" bit is set to 0
+        const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0);
+
+        if (opmode_self_test_cleared) {
+            // When self-test is successful, the corresponding self-test result bits are set
+            //  “X-Self-Test” register 0x42 bit0
+            //  “Y-Self-Test” register 0x44 bit0
+            //  “Z-Self-Test” register 0x46 bit0
+            const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0;
+            const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0;
+            const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0;
+
+            if (x_success && y_success && z_success) {
+                _state = STATE::READ_TRIM;
+                ScheduleDelayed(10_ms);
+
+            } else {
+                if (perf_event_count(_self_test_failed_perf) >= 5) {
+                    PX4_ERR("self test still failing after 5 attempts");
+
+                    // reluctantly proceed
+                    _state = STATE::READ_TRIM;
+                    ScheduleDelayed(10_ms);
+
+                } else {
+                    PX4_ERR("self test failed, resetting");
+                    perf_count(_self_test_failed_perf);
+                    _state = STATE::RESET;
+                    ScheduleDelayed(1_s);
+                }
+            }
+
+        } else {
+            if (hrt_elapsed_time(&_reset_timestamp) < 3_s) {
+                // self test not complete, check again in 100 milliseconds
+                _state = STATE::SELF_TEST_CHECK;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                // full reset
+                _state = STATE::RESET;
+                ScheduleDelayed(10_ms);
+            }
+        }
+    }
+
+    break;
+
+    case STATE::READ_TRIM: {
+        // Trim register value is read
+        uint8_t cmd = static_cast<uint8_t>(Register::DIG_X1);
+        uint8_t trim_x1y1[2]{};
+
+        if (transfer(&cmd, 1, trim_x1y1, 2) == PX4_OK) {
+            cmd = static_cast<uint8_t>(Register::DIG_Z4_LSB);
+            uint8_t trim_xyz_data[4]{};
+
+            if (transfer(&cmd, 1, trim_xyz_data, 4) == PX4_OK) {
+                cmd = static_cast<uint8_t>(Register::DIG_Z2_LSB);
+                uint8_t trim_xy1xy2[10]{};
+
+                if (transfer(&cmd, 1, trim_xy1xy2, 10) == PX4_OK) {
+                    _trim_data.dig_x1 = (int8_t)trim_x1y1[0];
+                    _trim_data.dig_y1 = (int8_t)trim_x1y1[1];
+
+                    _trim_data.dig_x2 = (int8_t)trim_xyz_data[2];
+                    _trim_data.dig_y2 = (int8_t)trim_xyz_data[3];
+
+                    uint16_t temp_msb;
+                    temp_msb          = ((uint16_t)trim_xy1xy2[3]) << 8;
+                    _trim_data.dig_z1 = (uint16_t)(temp_msb | trim_xy1xy2[2]);
+
+                    temp_msb          = ((uint16_t)trim_xy1xy2[1]) << 8;
+                    _trim_data.dig_z2 = (int16_t)(temp_msb | trim_xy1xy2[0]);
+
+                    temp_msb          = ((uint16_t)trim_xy1xy2[7]) << 8;
+                    _trim_data.dig_z3 = (int16_t)(temp_msb | trim_xy1xy2[6]);
+
+                    temp_msb          = ((uint16_t)trim_xyz_data[1]) << 8;
+                    _trim_data.dig_z4 = (int16_t)(temp_msb | trim_xyz_data[0]);
+
+                    _trim_data.dig_xy1 = trim_xy1xy2[9];
+                    _trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8];
+
+                    temp_msb            = ((uint16_t)(trim_xy1xy2[5] & 0x7F)) << 8;
+                    _trim_data.dig_xyz1 = (uint16_t)(temp_msb | trim_xy1xy2[4]);
+
+                    if ((_trim_data.dig_xyz1 != 0) && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0)) {
+                        _state = STATE::CONFIGURE;
+                        ScheduleDelayed(1_ms);
+                        return;
+                    }
+                }
+            }
+        }
+
+        // reset if reading trim failed
+        PX4_DEBUG("reading trim failed, resetting");
+        perf_count(_bad_register_perf);
+        _state = STATE::RESET;
+        ScheduleDelayed(100_ms);
+    }
+
+    break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start reading every 50 ms (20 Hz)
+            _state = STATE::READ;
+            ScheduleOnInterval(50_ms, 50_ms);
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::READ: {
+        struct TransferBuffer {
+            uint8_t DATAX_LSB;
+            uint8_t DATAX_MSB;
+            uint8_t DATAY_LSB;
+            uint8_t DATAY_MSB;
+            uint8_t DATAZ_LSB;
+            uint8_t DATAZ_MSB;
+            uint8_t RHALL_LSB;
+            uint8_t RHALL_MSB;
+            uint8_t STATUS;
+        } buffer{};
+
+        bool success = false;
+        // 0x42 to 0x4A with a burst read.
+        uint8_t cmd = static_cast<uint8_t>(Register::DATAX_LSB);
+
+        if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
+            int16_t  x     = combine_xy_int13(buffer.DATAX_MSB, buffer.DATAX_LSB);
+            int16_t  y     = combine_xy_int13(buffer.DATAY_MSB, buffer.DATAY_LSB);
+            int16_t  z     = combine_z_int15(buffer.DATAZ_MSB, buffer.DATAZ_LSB);
+            uint16_t rhall = combine_rhall_uint14(buffer.RHALL_MSB, buffer.RHALL_LSB);
+
+            const bool data_ready = buffer.RHALL_LSB & Bit0;
+
+            if (data_ready && (rhall != 0)) {
+                if ((buffer.STATUS & STATUS_BIT::Overflow) || (x == OVERFLOW_XYAXES) || (y == OVERFLOW_XYAXES) || (z == OVERFLOW_ZAXIS)) {
+                    // overflow ADC value, record error, but don't publish
+                    perf_count(_overflow_perf);
+
+                } else {
+                    _px4_mag.set_error_count(perf_event_count(_bad_register_perf)
+                                             + perf_event_count(_bad_transfer_perf) + perf_event_count(_self_test_failed_perf));
+                    _px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall));
+
+                    success = true;
+
+                    if (_failure_count > 0) {
+                        _failure_count--;
+                    }
+                }
+            }
+
+        } else {
+            perf_count(_bad_transfer_perf);
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+            }
+        }
+    }
+
+    break;
+    }
 }
 
-bool BMM150::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool BMM150::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	// microTesla -> Gauss
-	_px4_mag.set_scale(0.01f);
+    // microTesla -> Gauss
+    _px4_mag.set_scale(0.01f);
 
-	return success;
+    return success;
 }
 
-bool BMM150::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool BMM150::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t BMM150::RegisterRead(Register reg)
-{
-	const uint8_t cmd = static_cast<uint8_t>(reg);
-	uint8_t buffer{};
-	int ret = transfer(&cmd, 1, &buffer, 1);
+uint8_t BMM150::RegisterRead(Register reg) {
+    const uint8_t cmd = static_cast<uint8_t>(reg);
+    uint8_t       buffer{};
+    int           ret = transfer(&cmd, 1, &buffer, 1);
 
-	if (ret != PX4_OK) {
-		PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret);
-		return -1;
-	}
+    if (ret != PX4_OK) {
+        PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret);
+        return -1;
+    }
 
-	return buffer;
+    return buffer;
 }
 
-void BMM150::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t buffer[2] { (uint8_t)reg, value };
-	int ret = transfer(buffer, sizeof(buffer), nullptr, 0);
+void BMM150::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t buffer[2]{(uint8_t)reg, value};
+    int     ret = transfer(buffer, sizeof(buffer), nullptr, 0);
 
-	if (ret != PX4_OK) {
-		PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret);
-	}
+    if (ret != PX4_OK) {
+        PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret);
+    }
 }
 
-void BMM150::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+void BMM150::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
+    uint8_t       val      = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
diff --git a/apps/peripheral/sensor/mag/bmm150/BMM150.hpp b/apps/peripheral/sensor/mag/bmm150/BMM150.hpp
index 093a893d9d..0306c34447 100644
--- a/apps/peripheral/sensor/mag/bmm150/BMM150.hpp
+++ b/apps/peripheral/sensor/mag/bmm150/BMM150.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file BMM150.hpp
@@ -50,86 +27,90 @@
 
 using namespace Bosch_BMM150;
 
-class BMM150 : public device::I2C, public I2CSPIDriver<BMM150>
-{
+class BMM150 : public device::I2C, public I2CSPIDriver<BMM150> {
 public:
-	BMM150(const I2CSPIDriverConfig &config);
-	~BMM150() override;
+    BMM150(const I2CSPIDriverConfig &config);
+    ~BMM150() override;
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-
-	struct trim_registers {
-		int8_t dig_x1;     // trim x1 data
-		int8_t dig_y1;     // trim y1 data
-		int8_t dig_x2;     // trim x2 data
-		int8_t dig_y2;     // trim y2 data
-		uint16_t dig_z1;   // trim z1 data
-		int16_t dig_z2;    // trim z2 data
-		int16_t dig_z3;    // trim z3 data
-		int16_t dig_z4;    // trim z4 data
-		uint8_t dig_xy1;   // trim xy1 data
-		int8_t dig_xy2;    // trim xy2 data
-		uint16_t dig_xyz1; // trim xyz1 data
-	} _trim_data{};
-
-	// Sensor Configuration
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-	bool Reset();
-	bool Configure();
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-	void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); };
-	void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); };
-
-	// obtain the compensated magnetometer data in micro-tesla.
-	float compensate_x(int16_t mag_data_x, uint16_t data_rhall);
-	float compensate_y(int16_t mag_data_y, uint16_t data_rhall);
-	float compensate_z(int16_t mag_data_z, uint16_t data_rhall);
-
-	PX4Magnetometer _px4_mag;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": overflow")};
-	perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME": self test failed")};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	int _failure_count{0};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		SELF_TEST_CHECK,
-		READ_TRIM,
-		CONFIGURE,
-		READ,
-	} _state{STATE::RESET};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{4};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register                | Set bits, Clear bits
-		{ Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset },
-		{ Register::OP_MODE,       OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test },
-		{ Register::REPXY,         REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR },
-		{ Register::REPZ,          REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR },
-	};
+    struct trim_registers {
+        int8_t   dig_x1;   // trim x1 data
+        int8_t   dig_y1;   // trim y1 data
+        int8_t   dig_x2;   // trim x2 data
+        int8_t   dig_y2;   // trim y2 data
+        uint16_t dig_z1;   // trim z1 data
+        int16_t  dig_z2;   // trim z2 data
+        int16_t  dig_z3;   // trim z3 data
+        int16_t  dig_z4;   // trim z4 data
+        uint8_t  dig_xy1;  // trim xy1 data
+        int8_t   dig_xy2;  // trim xy2 data
+        uint16_t dig_xyz1; // trim xyz1 data
+    } _trim_data{};
+
+    // Sensor Configuration
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int  probe() override;
+    bool Reset();
+    bool Configure();
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    void RegisterClearBits(Register reg, uint8_t clearbits) {
+        RegisterSetAndClearBits(reg, 0, clearbits);
+    };
+
+    void RegisterSetBits(Register reg, uint8_t setbits) {
+        RegisterSetAndClearBits(reg, setbits, 0);
+    };
+
+    // obtain the compensated magnetometer data in micro-tesla.
+    float compensate_x(int16_t mag_data_x, uint16_t data_rhall);
+    float compensate_y(int16_t mag_data_y, uint16_t data_rhall);
+    float compensate_z(int16_t mag_data_z, uint16_t data_rhall);
+
+    PX4Magnetometer _px4_mag;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
+    perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")};
+    perf_counter_t _overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME ": overflow")};
+    perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")};
+
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_config_check_timestamp{0};
+    int         _failure_count{0};
+
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        SELF_TEST_CHECK,
+        READ_TRIM,
+        CONFIGURE,
+        READ,
+    } _state{STATE::RESET};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{4};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register                | Set bits, Clear bits
+        {Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset},
+        {Register::OP_MODE, OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test},
+        {Register::REPXY, REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR},
+        {Register::REPZ, REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR},
+    };
 };
diff --git a/apps/peripheral/sensor/mag/bmm150/Bosch_BMM150_registers.hpp b/apps/peripheral/sensor/mag/bmm150/Bosch_BMM150_registers.hpp
index 78c128ff4c..88740a1b7b 100644
--- a/apps/peripheral/sensor/mag/bmm150/Bosch_BMM150_registers.hpp
+++ b/apps/peripheral/sensor/mag/bmm150/Bosch_BMM150_registers.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file Bosch_BMM150_registers.hpp
@@ -52,10 +29,9 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace Bosch_BMM150
-{
-static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
-static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x10;
+namespace Bosch_BMM150 {
+static constexpr uint32_t I2C_SPEED           = 400 * 1000; // 400 kHz I2C serial interface
+static constexpr uint8_t  I2C_ADDRESS_DEFAULT = 0x10;
 
 static constexpr uint8_t chip_identification_number = 0x32;
 
@@ -63,64 +39,64 @@ static constexpr int16_t OVERFLOW_XYAXES = -4096;
 static constexpr int16_t OVERFLOW_ZAXIS  = -16384;
 
 enum class Register : uint8_t {
-	CHIP_ID       = 0x40, // magnetometer chip identification number
+    CHIP_ID = 0x40,       // magnetometer chip identification number
 
-	DATAX_LSB     = 0x42, // 5-bit LSB of x-axis magnetic field data
-	DATAX_MSB     = 0x43, // 8-bit MSB of x-axis magnetic field data
-	DATAY_LSB     = 0x44, // 5-bit LSB of y-axis magnetic field data
-	DATAY_MSB     = 0x45, // 8-bit MSB of y-axis magnetic field data
-	DATAZ_LSB     = 0x46, // 7-bit LSB of z-axis magnetic field data
-	DATAZ_MSB     = 0x47, // 8-bit MSB of z-axis magnetic field data
-	RHALL_LSB     = 0x48, // 6-bit LSB of hall resistance
-	RHALL_MSB     = 0x49, // 8-bit MSB of hall resistance
-	STATUS        = 0x4A, // Status register
-	POWER_CONTROL = 0x4B, // power control, soft reset and interface SPI mode selection
-	OP_MODE       = 0x4C, // operation mode, output data rate and self-test
+    DATAX_LSB     = 0x42, // 5-bit LSB of x-axis magnetic field data
+    DATAX_MSB     = 0x43, // 8-bit MSB of x-axis magnetic field data
+    DATAY_LSB     = 0x44, // 5-bit LSB of y-axis magnetic field data
+    DATAY_MSB     = 0x45, // 8-bit MSB of y-axis magnetic field data
+    DATAZ_LSB     = 0x46, // 7-bit LSB of z-axis magnetic field data
+    DATAZ_MSB     = 0x47, // 8-bit MSB of z-axis magnetic field data
+    RHALL_LSB     = 0x48, // 6-bit LSB of hall resistance
+    RHALL_MSB     = 0x49, // 8-bit MSB of hall resistance
+    STATUS        = 0x4A, // Status register
+    POWER_CONTROL = 0x4B, // power control, soft reset and interface SPI mode selection
+    OP_MODE       = 0x4C, // operation mode, output data rate and self-test
 
-	REPXY         = 0x51, // number of repetitions for x/y-axis
-	REPZ          = 0x52, // number of repetitions for z-axis
+    REPXY = 0x51,         // number of repetitions for x/y-axis
+    REPZ  = 0x52,         // number of repetitions for z-axis
 
-	DIG_X1        = 0x5D,
+    DIG_X1 = 0x5D,
 
-	DIG_Z4_LSB    = 0x62,
+    DIG_Z4_LSB = 0x62,
 
-	DIG_Z2_LSB    = 0x68,
+    DIG_Z2_LSB = 0x68,
 };
 
 // POWER_CONTROL
 enum POWER_CONTROL_BIT : uint8_t {
-	SoftReset    = Bit7 | Bit1, // soft reset trigger bits.
-	PowerControl = Bit0,        // When set to “0”, suspend mode is selected
+    SoftReset    = Bit7 | Bit1, // soft reset trigger bits.
+    PowerControl = Bit0,        // When set to “0”, suspend mode is selected
 };
 
 // OP_MODE
 enum OP_MODE_BIT : uint8_t {
-	// 5:3 Data rate control
-	ODR_20HZ_SET   = Bit5 | Bit3,
-	ODR_20HZ_CLEAR = Bit4,
+    // 5:3 Data rate control
+    ODR_20HZ_SET   = Bit5 | Bit3,
+    ODR_20HZ_CLEAR = Bit4,
 
-	// 2:1 Operation mode control
-	Opmode_Sleep   = Bit2 | Bit1, // Sleep mode
-	Self_Test      = Bit0,
+    // 2:1 Operation mode control
+    Opmode_Sleep = Bit2 | Bit1, // Sleep mode
+    Self_Test    = Bit0,
 };
 
 // STATUS
 enum STATUS_BIT : uint8_t {
-	Overflow = Bit6, // one or more axes exceeded maximum range of the device
+    Overflow = Bit6, // one or more axes exceeded maximum range of the device
 };
 
 // REPXY
 enum REPXY_BIT : uint8_t {
-	// high accurary preset nXY = 47, REPXY = 0x17 = 0b0001'0111
-	XY_HA_SET   = Bit4 | Bit2 | Bit1 | Bit0,
-	XY_HA_CLEAR = Bit7 | Bit6 | Bit5 | Bit3,
+    // high accurary preset nXY = 47, REPXY = 0x17 = 0b0001'0111
+    XY_HA_SET   = Bit4 | Bit2 | Bit1 | Bit0,
+    XY_HA_CLEAR = Bit7 | Bit6 | Bit5 | Bit3,
 };
 
 // REPZ
 enum REPZ_BIT : uint8_t {
-	// high accurary preset nZ = 83, REPZ = 0x52 = 0b0101'0010
-	Z_HA_SET   = Bit6 | Bit4 | Bit1,
-	Z_HA_CLEAR = Bit7 | Bit5 | Bit3 | Bit2 | Bit0,
+    // high accurary preset nZ = 83, REPZ = 0x52 = 0b0101'0010
+    Z_HA_SET   = Bit6 | Bit4 | Bit1,
+    Z_HA_CLEAR = Bit7 | Bit5 | Bit3 | Bit2 | Bit0,
 };
 
 } // namespace Bosch_BMM150
diff --git a/apps/peripheral/sensor/mag/bmm150/bmm150_main.cpp b/apps/peripheral/sensor/mag/bmm150/bmm150_main.cpp
index 50ad12df3a..2a9caad7c6 100644
--- a/apps/peripheral/sensor/mag/bmm150/bmm150_main.cpp
+++ b/apps/peripheral/sensor/mag/bmm150/bmm150_main.cpp
@@ -1,89 +1,64 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "BMM150.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void BMM150::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("bmm150", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x10);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void BMM150::print_usage() {
+    PRINT_MODULE_USAGE_NAME("bmm150", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x10);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int bmm150_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = BMM150;
-	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = I2C_SPEED;
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
+extern "C" int bmm150_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = BMM150;
+    BusCLIArguments cli{true, false};
+    cli.default_i2c_frequency = I2C_SPEED;
+    cli.i2c_address           = I2C_ADDRESS_DEFAULT;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+        switch (ch) {
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM150);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM150);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/mag/ist8310/IST8310.cpp b/apps/peripheral/sensor/mag/ist8310/IST8310.cpp
index b7d8512ce9..c68dec61f6 100644
--- a/apps/peripheral/sensor/mag/ist8310/IST8310.cpp
+++ b/apps/peripheral/sensor/mag/ist8310/IST8310.cpp
@@ -1,298 +1,260 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "IST8310.hpp"
 
 using namespace time_literals;
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
 }
 
 IST8310::IST8310(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config),
-	_px4_mag(get_device_id(), config.rotation)
-{
+    I2C(config),
+    I2CSPIDriver(config),
+    _px4_mag(get_device_id(), config.rotation) {
 }
 
-IST8310::~IST8310()
-{
-	perf_free(_reset_perf);
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
+IST8310::~IST8310() {
+    perf_free(_reset_perf);
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
 }
 
-int IST8310::init()
-{
-	int ret = I2C::init();
+int IST8310::init() {
+    int ret = I2C::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("I2C::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("I2C::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool IST8310::Reset()
-{
-	_state = STATE::RESET;
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool IST8310::Reset() {
+    _state = STATE::RESET;
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
 
-void IST8310::print_status()
-{
-	I2CSPIDriverBase::print_status();
+void IST8310::print_status() {
+    I2CSPIDriverBase::print_status();
 
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_reset_perf);
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
 }
 
-int IST8310::probe()
-{
-	const uint8_t WAI = RegisterRead(Register::WAI);
+int IST8310::probe() {
+    const uint8_t WAI = RegisterRead(Register::WAI);
 
-	if (WAI != Device_ID) {
-		DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
-		return PX4_ERROR;
-	}
+    if (WAI != Device_ID) {
+        DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
+        return PX4_ERROR;
+    }
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-void IST8310::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// CNTL2: Software Reset
-		RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		perf_count(_reset_perf);
-		ScheduleDelayed(50_ms); // Power On Reset: max 50ms
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-
-		// SRST: This bit is automatically reset to zero after POR routine
-		if ((RegisterRead(Register::WAI) == Device_ID)
-		    && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SRST) == 0)) {
-
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms);
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 10 ms");
-				ScheduleDelayed(10_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start measurement cycle
-			_state = STATE::MEASURE;
-			ScheduleDelayed(20_ms);
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::MEASURE:
-		RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT);
-		_state = STATE::READ;
-		ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup)
-		break;
-
-	case STATE::READ: {
-			struct TransferBuffer {
-				uint8_t STAT1;
-				uint8_t DATAXL;
-				uint8_t DATAXH;
-				uint8_t DATAYL;
-				uint8_t DATAYH;
-				uint8_t DATAZL;
-				uint8_t DATAZH;
-			} buffer{};
-
-			bool success = false;
-			uint8_t cmd = static_cast<uint8_t>(Register::STAT1);
-
-			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
-
-				if (buffer.STAT1 & STAT1_BIT::DRDY) {
-					int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
-					int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
-					int16_t z = combine(buffer.DATAZH, buffer.DATAZL);
-
-					// sensor's frame is +x forward, +y right, +z up
-					z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
-
-					_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
-					_px4_mag.update(now, x, y, z);
-
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-
-			} else {
-				perf_count(_bad_transfer_perf);
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-					return;
-				}
-			}
-
-			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
-				// check configuration registers periodically or immediately following any failure
-				if (RegisterCheck(_register_cfg[_checked_register])) {
-					_last_config_check_timestamp = now;
-					_checked_register = (_checked_register + 1) % size_register_cfg;
-
-				} else {
-					// register check failed, force reset
-					perf_count(_bad_register_perf);
-					Reset();
-					return;
-				}
-			}
-
-			// initiate next measurement
-			RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT);
-			ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup)
-		}
-
-		break;
-	}
+void IST8310::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // CNTL2: Software Reset
+        RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        perf_count(_reset_perf);
+        ScheduleDelayed(50_ms); // Power On Reset: max 50ms
+        break;
+
+    case STATE::WAIT_FOR_RESET:
+
+        // SRST: This bit is automatically reset to zero after POR routine
+        if ((RegisterRead(Register::WAI) == Device_ID)
+            && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SRST) == 0)) {
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleDelayed(10_ms);
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 10 ms");
+                ScheduleDelayed(10_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start measurement cycle
+            _state = STATE::MEASURE;
+            ScheduleDelayed(20_ms);
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::MEASURE:
+        RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT);
+        _state = STATE::READ;
+        ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup)
+        break;
+
+    case STATE::READ: {
+        struct TransferBuffer {
+            uint8_t STAT1;
+            uint8_t DATAXL;
+            uint8_t DATAXH;
+            uint8_t DATAYL;
+            uint8_t DATAYH;
+            uint8_t DATAZL;
+            uint8_t DATAZH;
+        } buffer{};
+
+        bool    success = false;
+        uint8_t cmd     = static_cast<uint8_t>(Register::STAT1);
+
+        if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
+            if (buffer.STAT1 & STAT1_BIT::DRDY) {
+                int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
+                int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
+                int16_t z = combine(buffer.DATAZH, buffer.DATAZL);
+
+                // sensor's frame is +x forward, +y right, +z up
+                z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
+
+                _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
+                _px4_mag.update(now, x, y, z);
+
+                success = true;
+
+                if (_failure_count > 0) {
+                    _failure_count--;
+                }
+            }
+
+        } else {
+            perf_count(_bad_transfer_perf);
+        }
+
+        if (!success) {
+            _failure_count++;
+
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+                return;
+            }
+        }
+
+        if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+            // check configuration registers periodically or immediately following any failure
+            if (RegisterCheck(_register_cfg[_checked_register])) {
+                _last_config_check_timestamp = now;
+                _checked_register            = (_checked_register + 1) % size_register_cfg;
+
+            } else {
+                // register check failed, force reset
+                perf_count(_bad_register_perf);
+                Reset();
+                return;
+            }
+        }
+
+        // initiate next measurement
+        RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT);
+        ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup)
+    }
+
+    break;
+    }
 }
 
-bool IST8310::Configure()
-{
-	// first set and clear all configured register bits
-	for (const auto &reg_cfg : _register_cfg) {
-		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
-	}
+bool IST8310::Configure() {
+    // first set and clear all configured register bits
+    for (const auto &reg_cfg : _register_cfg) {
+        RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+    }
 
-	// now check that all are configured
-	bool success = true;
+    // now check that all are configured
+    bool success = true;
 
-	for (const auto &reg_cfg : _register_cfg) {
-		if (!RegisterCheck(reg_cfg)) {
-			success = false;
-		}
-	}
+    for (const auto &reg_cfg : _register_cfg) {
+        if (!RegisterCheck(reg_cfg)) {
+            success = false;
+        }
+    }
 
-	_px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
+    _px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
 
-	return success;
+    return success;
 }
 
-bool IST8310::RegisterCheck(const register_config_t &reg_cfg)
-{
-	bool success = true;
+bool IST8310::RegisterCheck(const register_config_t &reg_cfg) {
+    bool success = true;
 
-	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+    const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
-	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		success = false;
-	}
+    if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+        success = false;
+    }
 
-	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
-		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		success = false;
-	}
+    if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+        PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+        success = false;
+    }
 
-	return success;
+    return success;
 }
 
-uint8_t IST8310::RegisterRead(Register reg)
-{
-	const uint8_t cmd = static_cast<uint8_t>(reg);
-	uint8_t buffer{};
-	transfer(&cmd, 1, &buffer, 1);
-	return buffer;
+uint8_t IST8310::RegisterRead(Register reg) {
+    const uint8_t cmd = static_cast<uint8_t>(reg);
+    uint8_t       buffer{};
+    transfer(&cmd, 1, &buffer, 1);
+    return buffer;
 }
 
-void IST8310::RegisterWrite(Register reg, uint8_t value)
-{
-	uint8_t buffer[2] { (uint8_t)reg, value };
-	transfer(buffer, sizeof(buffer), nullptr, 0);
+void IST8310::RegisterWrite(Register reg, uint8_t value) {
+    uint8_t buffer[2]{(uint8_t)reg, value};
+    transfer(buffer, sizeof(buffer), nullptr, 0);
 }
 
-void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
-{
-	const uint8_t orig_val = RegisterRead(reg);
-	uint8_t val = (orig_val & ~clearbits) | setbits;
+void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) {
+    const uint8_t orig_val = RegisterRead(reg);
+    uint8_t       val      = (orig_val & ~clearbits) | setbits;
 
-	if (orig_val != val) {
-		RegisterWrite(reg, val);
-	}
+    if (orig_val != val) {
+        RegisterWrite(reg, val);
+    }
 }
diff --git a/apps/peripheral/sensor/mag/ist8310/IST8310.hpp b/apps/peripheral/sensor/mag/ist8310/IST8310.hpp
index a251f49cf0..de4976c9ad 100644
--- a/apps/peripheral/sensor/mag/ist8310/IST8310.hpp
+++ b/apps/peripheral/sensor/mag/ist8310/IST8310.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file IST8310.hpp
@@ -50,64 +27,63 @@
 
 using namespace iSentek_IST8310;
 
-class IST8310 : public device::I2C, public I2CSPIDriver<IST8310>
-{
+class IST8310 : public device::I2C, public I2CSPIDriver<IST8310> {
 public:
-	IST8310(const I2CSPIDriverConfig &config);
-	~IST8310() override;
+    IST8310(const I2CSPIDriverConfig &config);
+    ~IST8310() override;
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	// Sensor Configuration
-	struct register_config_t {
-		Register reg;
-		uint8_t set_bits{0};
-		uint8_t clear_bits{0};
-	};
-
-	int probe() override;
-
-	bool Reset();
-
-	bool Configure();
-
-	bool RegisterCheck(const register_config_t &reg_cfg);
-
-	uint8_t RegisterRead(Register reg);
-	void RegisterWrite(Register reg, uint8_t value);
-	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-
-	PX4Magnetometer _px4_mag;
-
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_config_check_timestamp{0};
-	int _failure_count{0};
-
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		MEASURE,
-		READ,
-	} _state{STATE::RESET};
-
-	uint8_t _checked_register{0};
-	static constexpr uint8_t size_register_cfg{4};
-	register_config_t _register_cfg[size_register_cfg] {
-		// Register               | Set bits, Clear bits
-		{ Register::CNTL2,        0, CNTL2_BIT::SRST },
-		{ Register::CNTL3,        CNTL3_BIT::Z_16BIT | CNTL3_BIT::Y_16BIT | CNTL3_BIT::X_16BIT, 0 },
-		{ Register::AVGCNTL,      AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR },
-		{ Register::PDCNTL,       PDCNTL_BIT::PULSE_NORMAL, 0 },
-	};
+    // Sensor Configuration
+    struct register_config_t {
+        Register reg;
+        uint8_t  set_bits{0};
+        uint8_t  clear_bits{0};
+    };
+
+    int probe() override;
+
+    bool Reset();
+
+    bool Configure();
+
+    bool RegisterCheck(const register_config_t &reg_cfg);
+
+    uint8_t RegisterRead(Register reg);
+    void    RegisterWrite(Register reg, uint8_t value);
+    void    RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+    PX4Magnetometer _px4_mag;
+
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
+    perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")};
+
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_config_check_timestamp{0};
+    int         _failure_count{0};
+
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        MEASURE,
+        READ,
+    } _state{STATE::RESET};
+
+    uint8_t                  _checked_register{0};
+    static constexpr uint8_t size_register_cfg{4};
+    register_config_t        _register_cfg[size_register_cfg]{
+        // Register               | Set bits, Clear bits
+        {Register::CNTL2, 0, CNTL2_BIT::SRST},
+        {Register::CNTL3, CNTL3_BIT::Z_16BIT | CNTL3_BIT::Y_16BIT | CNTL3_BIT::X_16BIT, 0},
+        {Register::AVGCNTL, AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR},
+        {Register::PDCNTL, PDCNTL_BIT::PULSE_NORMAL, 0},
+    };
 };
diff --git a/apps/peripheral/sensor/mag/ist8310/iSentek_IST8310_registers.hpp b/apps/peripheral/sensor/mag/ist8310/iSentek_IST8310_registers.hpp
index 2a49ee03e9..f2549cfc6b 100644
--- a/apps/peripheral/sensor/mag/ist8310/iSentek_IST8310_registers.hpp
+++ b/apps/peripheral/sensor/mag/ist8310/iSentek_IST8310_registers.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file iSentek_IST8310_registers.hpp
@@ -52,100 +29,99 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace iSentek_IST8310
-{
-static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
-static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x0E;
+namespace iSentek_IST8310 {
+static constexpr uint32_t I2C_SPEED           = 400 * 1000; // 400 kHz I2C serial interface
+static constexpr uint8_t  I2C_ADDRESS_DEFAULT = 0x0E;
 
 static constexpr uint8_t Device_ID = 0x10;
 
 enum class Register : uint8_t {
-	WAI        = 0x00, // Who Am I Register
-
-	STAT1      = 0x02, // Status Register 1
-	DATAXL     = 0x03,
-	DATAXH     = 0x04,
-	DATAYL     = 0x05,
-	DATAYH     = 0x06,
-	DATAZL     = 0x07,
-	DATAZH     = 0x08,
-	STAT2      = 0x09, // Status Register 2
-	CNTL1      = 0x0A, // Control Setting Register 1
-	CNTL2      = 0x0B, // Control Setting Register 2
-	STR        = 0x0C, // Self-Test Register
-	CNTL3      = 0x0D, // Control Setting Register 3
-
-	TEMPL      = 0x1C, // Temperature Sensor Output Register (Low Byte)
-	TEMPH      = 0x1D, // Temperature Sensor Output Register (High Byte)
-
-	TCCNTL     = 0x40, // Temperature Compensation Control register
-	AVGCNTL    = 0x41, // Average Control Register
-	PDCNTL     = 0x42, // Pulse Duration Control Register
-
-	XX_CROSS_L = 0x9C, // cross axis xx low byte
-	XX_CROSS_H = 0x9D, // cross axis xx high byte
-	XY_CROSS_L = 0x9E, // cross axis xy low byte
-	XY_CROSS_H = 0x9F, // cross axis xy high byte
-	XZ_CROSS_L = 0xA0, // cross axis xz low byte
-	XZ_CROSS_H = 0xA1, // cross axis xz high byte
-	YX_CROSS_L = 0xA2, // cross axis yx low byte
-	YX_CROSS_H = 0xA3, // cross axis yx high byte
-	YY_CROSS_L = 0xA4, // cross axis yy low byte
-	YY_CROSS_H = 0xA5, // cross axis yy high byte
-	YZ_CROSS_L = 0xA6, // cross axis yz low byte
-	YZ_CROSS_H = 0xA7, // cross axis yz high byte
-	ZX_CROSS_L = 0xA8, // cross axis zx low byte
-	ZX_CROSS_H = 0xA9, // cross axis zx high byte
-	ZY_CROSS_L = 0xAA, // cross axis zy low byte
-	ZY_CROSS_H = 0xAB, // cross axis zy high byte
-	ZZ_CROSS_L = 0xAC, // cross axis zz low byte
-	ZZ_CROSS_H = 0xAD, // cross axis zz high byte
+    WAI = 0x00,    // Who Am I Register
+
+    STAT1  = 0x02, // Status Register 1
+    DATAXL = 0x03,
+    DATAXH = 0x04,
+    DATAYL = 0x05,
+    DATAYH = 0x06,
+    DATAZL = 0x07,
+    DATAZH = 0x08,
+    STAT2  = 0x09,     // Status Register 2
+    CNTL1  = 0x0A,     // Control Setting Register 1
+    CNTL2  = 0x0B,     // Control Setting Register 2
+    STR    = 0x0C,     // Self-Test Register
+    CNTL3  = 0x0D,     // Control Setting Register 3
+
+    TEMPL = 0x1C,      // Temperature Sensor Output Register (Low Byte)
+    TEMPH = 0x1D,      // Temperature Sensor Output Register (High Byte)
+
+    TCCNTL  = 0x40,    // Temperature Compensation Control register
+    AVGCNTL = 0x41,    // Average Control Register
+    PDCNTL  = 0x42,    // Pulse Duration Control Register
+
+    XX_CROSS_L = 0x9C, // cross axis xx low byte
+    XX_CROSS_H = 0x9D, // cross axis xx high byte
+    XY_CROSS_L = 0x9E, // cross axis xy low byte
+    XY_CROSS_H = 0x9F, // cross axis xy high byte
+    XZ_CROSS_L = 0xA0, // cross axis xz low byte
+    XZ_CROSS_H = 0xA1, // cross axis xz high byte
+    YX_CROSS_L = 0xA2, // cross axis yx low byte
+    YX_CROSS_H = 0xA3, // cross axis yx high byte
+    YY_CROSS_L = 0xA4, // cross axis yy low byte
+    YY_CROSS_H = 0xA5, // cross axis yy high byte
+    YZ_CROSS_L = 0xA6, // cross axis yz low byte
+    YZ_CROSS_H = 0xA7, // cross axis yz high byte
+    ZX_CROSS_L = 0xA8, // cross axis zx low byte
+    ZX_CROSS_H = 0xA9, // cross axis zx high byte
+    ZY_CROSS_L = 0xAA, // cross axis zy low byte
+    ZY_CROSS_H = 0xAB, // cross axis zy high byte
+    ZZ_CROSS_L = 0xAC, // cross axis zz low byte
+    ZZ_CROSS_H = 0xAD, // cross axis zz high byte
 };
 
 // STAT1
 enum STAT1_BIT : uint8_t {
-	DOR  = Bit1, // Data overrun bit
-	DRDY = Bit0, // Data ready
+    DOR  = Bit1, // Data overrun bit
+    DRDY = Bit0, // Data ready
 };
 
 // CNTL1
 enum CNTL1_BIT : uint8_t {
-	// 3:0 Mode: Operating mode setting
-	MODE_SINGLE_MEASUREMENT = Bit0,
+    // 3:0 Mode: Operating mode setting
+    MODE_SINGLE_MEASUREMENT = Bit0,
 };
 
 // CNTL2
 enum CNTL2_BIT : uint8_t {
-	SRST = Bit0, // Soft reset, perform the same routine as POR
+    SRST = Bit0, // Soft reset, perform the same routine as POR
 };
 
 // STR
 enum STR_BIT : uint8_t {
-	SELF_TEST = Bit6,
+    SELF_TEST = Bit6,
 };
 
 // CNTL3
 enum CNTL3_BIT : uint8_t {
-	Z_16BIT = Bit6, // Sensor output resolution adjustment for Z axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
-	Y_16BIT = Bit5, // Sensor output resolution adjustment for Y axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
-	X_16BIT = Bit4, // Sensor output resolution adjustment for X axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
+    Z_16BIT = Bit6, // Sensor output resolution adjustment for Z axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
+    Y_16BIT = Bit5, // Sensor output resolution adjustment for Y axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
+    X_16BIT = Bit4, // Sensor output resolution adjustment for X axis: 16-bit (Sensitivity: 1320 LSB/Gauss)
 };
 
 // AVGCNTL
 enum AVGCNTL_BIT : uint8_t {
-	// 5:3 Average times for y sensor data. Times of average will be done before switch to next channel
-	Y_16TIMES_SET   = Bit5, // 3’b100 average by 16 times (ODRmax=166Hz)
-	Y_16TIMES_CLEAR = Bit4 | Bit3,
+    // 5:3 Average times for y sensor data. Times of average will be done before switch to next channel
+    Y_16TIMES_SET   = Bit5, // 3’b100 average by 16 times (ODRmax=166Hz)
+    Y_16TIMES_CLEAR = Bit4 | Bit3,
 
-	// 2:0 Average times for x & z sensor data. Times of average will be done before switch to next channel
-	XZ_16TIMES_SET   = Bit2, // average by 16 times (ODRmax=166Hz)
-	XZ_16TIMES_CLEAR = Bit1 | Bit0,
+    // 2:0 Average times for x & z sensor data. Times of average will be done before switch to next channel
+    XZ_16TIMES_SET   = Bit2, // average by 16 times (ODRmax=166Hz)
+    XZ_16TIMES_CLEAR = Bit1 | Bit0,
 };
 
 // PDCNTL
 enum PDCNTL_BIT : uint8_t {
-	// 7:6 Pulse duration
-	PULSE_NORMAL = Bit7 | Bit6, // Normal (please use this setting)
+    // 7:6 Pulse duration
+    PULSE_NORMAL = Bit7 | Bit6, // Normal (please use this setting)
 };
 
 } // namespace iSentek_IST8310
diff --git a/apps/peripheral/sensor/mag/ist8310/ist8310_main.cpp b/apps/peripheral/sensor/mag/ist8310/ist8310_main.cpp
index 098e78f891..e93756ef41 100644
--- a/apps/peripheral/sensor/mag/ist8310/ist8310_main.cpp
+++ b/apps/peripheral/sensor/mag/ist8310/ist8310_main.cpp
@@ -1,89 +1,64 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "IST8310.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void IST8310::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("ist8310", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0E);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void IST8310::print_usage() {
+    PRINT_MODULE_USAGE_NAME("ist8310", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x0E);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int ist8310_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = IST8310;
-	BusCLIArguments cli{true, false};
-	cli.i2c_address = I2C_ADDRESS_DEFAULT;
-	cli.default_i2c_frequency = I2C_SPEED;
+extern "C" int ist8310_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = IST8310;
+    BusCLIArguments cli{true, false};
+    cli.i2c_address           = I2C_ADDRESS_DEFAULT;
+    cli.default_i2c_frequency = I2C_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+        switch (ch) {
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IST8310);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IST8310);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/mag/rm3100/rm3100.cpp b/apps/peripheral/sensor/mag/rm3100/rm3100.cpp
index cf43608f8f..80af0a4e4d 100644
--- a/apps/peripheral/sensor/mag/rm3100/rm3100.cpp
+++ b/apps/peripheral/sensor/mag/rm3100/rm3100.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rm3100.cpp
@@ -42,242 +19,229 @@
 #include "rm3100.h"
 
 RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
-	I2CSPIDriver(config),
-	_px4_mag(interface->get_device_id(), config.rotation),
-	_interface(interface)
-{
-	_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
+    I2CSPIDriver(config),
+    _px4_mag(interface->get_device_id(), config.rotation),
+    _interface(interface) {
+    _px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
 }
 
-RM3100::~RM3100()
-{
-	// free perf counters
-	perf_free(_reset_perf);
-	perf_free(_range_error_perf);
-	perf_free(_bad_transfer_perf);
+RM3100::~RM3100() {
+    // free perf counters
+    perf_free(_reset_perf);
+    perf_free(_range_error_perf);
+    perf_free(_bad_transfer_perf);
 
-	delete _interface;
+    delete _interface;
 }
 
-int RM3100::self_test()
-{
-	bool complete = false;
-
-	// Set the default command mode and enable polling (not continuous mode)
-	uint8_t cmd = (CMM_DEFAULT & ~CONTINUOUS_MODE);
-	int ret = _interface->write(ADDR_CMM, &cmd, 1);
+int RM3100::self_test() {
+    bool complete = false;
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    // Set the default command mode and enable polling (not continuous mode)
+    uint8_t cmd = (CMM_DEFAULT & ~CONTINUOUS_MODE);
+    int     ret = _interface->write(ADDR_CMM, &cmd, 1);
 
-	cmd = HSHAKE_NO_DRDY_CLEAR;
-	ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    cmd = HSHAKE_NO_DRDY_CLEAR;
+    ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
 
-	// Configure sensor to execute BIST upon receipt of a POLL command
-	cmd = BIST_SELFTEST;
-	ret = _interface->write(ADDR_BIST, &cmd, 1);
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    // Configure sensor to execute BIST upon receipt of a POLL command
+    cmd = BIST_SELFTEST;
+    ret = _interface->write(ADDR_BIST, &cmd, 1);
 
-	// Poll to start the self test
-	cmd = POLL_XYZ;
-	ret = _interface->write(ADDR_POLL, &cmd, 1);
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	if (ret != PX4_OK) {
-		return ret;
-	}
+    // Poll to start the self test
+    cmd = POLL_XYZ;
+    ret = _interface->write(ADDR_POLL, &cmd, 1);
 
-	// Perform test procedure until a valid result is obtained or test times out
+    if (ret != PX4_OK) {
+        return ret;
+    }
 
-	const hrt_abstime t_start = hrt_absolute_time();
+    // Perform test procedure until a valid result is obtained or test times out
 
-	while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {
+    const hrt_abstime t_start = hrt_absolute_time();
 
-		uint8_t status = 0;
-		ret = _interface->read(ADDR_STATUS, &status, 1);
+    while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {
+        uint8_t status = 0;
+        ret            = _interface->read(ADDR_STATUS, &status, 1);
 
-		if (ret != PX4_OK) {
-			return ret;
-		}
+        if (ret != PX4_OK) {
+            return ret;
+        }
 
-		// If the DRDY bit in the status register is set, BIST should be complete
-		if (status & STATUS_DRDY) {
-			// Check BIST register to evaluate the test result
-			ret = _interface->read(ADDR_BIST, &cmd, 1);
+        // If the DRDY bit in the status register is set, BIST should be complete
+        if (status & STATUS_DRDY) {
+            // Check BIST register to evaluate the test result
+            ret = _interface->read(ADDR_BIST, &cmd, 1);
 
-			if (ret != PX4_OK) {
-				return ret;
-			}
+            if (ret != PX4_OK) {
+                return ret;
+            }
 
-			// The test results are not valid if STE is not set. In this case, we try again
-			if (cmd & BIST_STE) {
-				complete = true;
+            // The test results are not valid if STE is not set. In this case, we try again
+            if (cmd & BIST_STE) {
+                complete = true;
 
-				// If the x, y, or z LR oscillators malfunctioned then the self test failed.
-				if ((cmd & BIST_XYZ_OK) ^ BIST_XYZ_OK) {
-					PX4_ERR("built-in self test failed: 0x%2X x:%s y:%s z:%s", cmd,
-						cmd & 0x10 ? "Pass" : "Fail",
-						cmd & 0x20 ? "Pass" : "Fail",
-						cmd & 0x40 ? "Pass" : "Fail");
-					return PX4_ERROR;
+                // If the x, y, or z LR oscillators malfunctioned then the self test failed.
+                if ((cmd & BIST_XYZ_OK) ^ BIST_XYZ_OK) {
+                    PX4_ERR("built-in self test failed: 0x%2X x:%s y:%s z:%s", cmd,
+                            cmd & 0x10 ? "Pass" : "Fail",
+                            cmd & 0x20 ? "Pass" : "Fail",
+                            cmd & 0x40 ? "Pass" : "Fail");
+                    return PX4_ERROR;
 
-				} else {
-					// The test passed, disable self-test mode by clearing the STE bit
-					cmd = 0;
-					ret = _interface->write(ADDR_BIST, &cmd, 1);
+                } else {
+                    // The test passed, disable self-test mode by clearing the STE bit
+                    cmd = 0;
+                    ret = _interface->write(ADDR_BIST, &cmd, 1);
 
-					if (ret != PX4_OK) {
-						PX4_ERR("Failed to disable built-in self test");
-					}
+                    if (ret != PX4_OK) {
+                        PX4_ERR("Failed to disable built-in self test");
+                    }
 
-					return PX4_OK;
-				}
-			}
-		}
-	}
+                    return PX4_OK;
+                }
+            }
+        }
+    }
 
-	if (!complete) {
-		PX4_ERR("built-in self test incomplete");
-	}
+    if (!complete) {
+        PX4_ERR("built-in self test incomplete");
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void RM3100::RunImpl()
-{
-	// full reset if things are failing consistently
-	if (_failure_count > 10) {
-		_failure_count = 0;
-		set_default_register_values();
-		ScheduleOnInterval(RM3100_INTERVAL);
-		return;
-	}
-
-	struct {
-		uint8_t x[3];
-		uint8_t y[3];
-		uint8_t z[3];
-	} rm_report{};
-
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-	int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
-
-	if (ret != OK) {
-		perf_count(_bad_transfer_perf);
-		_failure_count++;
-		return;
-	}
-
-	/* Rearrange mag data */
-	int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
-	int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
-	int32_t zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]);
-
-	/* Convert 24 bit signed values to 32 bit signed values */
-	convert_signed(&xraw);
-	convert_signed(&yraw);
-	convert_signed(&zraw);
-
-	// valid range: -8388608 to 8388607
-	if (xraw < -8388608 || xraw > 8388607 ||
-	    yraw < -8388608 || yraw > 8388607 ||
-	    zraw < -8388608 || zraw > 8388607) {
-
-		_failure_count++;
-
-		perf_count(_range_error_perf);
-		return;
-	}
-
-	// only publish changes
-	if (_raw_data_prev[0] != xraw || _raw_data_prev[1] != yraw || _raw_data_prev[2] != zraw) {
-
-		_px4_mag.set_error_count(perf_event_count(_bad_transfer_perf)
-					 + perf_event_count(_range_error_perf));
-
-		_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
-
-		_raw_data_prev[0] = xraw;
-		_raw_data_prev[1] = yraw;
-		_raw_data_prev[2] = zraw;
-
-		if (_failure_count > 0) {
-			_failure_count--;
-		}
-
-	} else {
-		_failure_count++;
-	}
+void RM3100::RunImpl() {
+    // full reset if things are failing consistently
+    if (_failure_count > 10) {
+        _failure_count = 0;
+        set_default_register_values();
+        ScheduleOnInterval(RM3100_INTERVAL);
+        return;
+    }
+
+    struct {
+        uint8_t x[3];
+        uint8_t y[3];
+        uint8_t z[3];
+    } rm_report{};
+
+    const hrt_abstime timestamp_sample = hrt_absolute_time();
+    int               ret              = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
+
+    if (ret != OK) {
+        perf_count(_bad_transfer_perf);
+        _failure_count++;
+        return;
+    }
+
+    /* Rearrange mag data */
+    int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
+    int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
+    int32_t zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]);
+
+    /* Convert 24 bit signed values to 32 bit signed values */
+    convert_signed(&xraw);
+    convert_signed(&yraw);
+    convert_signed(&zraw);
+
+    // valid range: -8388608 to 8388607
+    if (xraw < -8388608 || xraw > 8388607 || yraw < -8388608 || yraw > 8388607 || zraw < -8388608 || zraw > 8388607) {
+        _failure_count++;
+
+        perf_count(_range_error_perf);
+        return;
+    }
+
+    // only publish changes
+    if (_raw_data_prev[0] != xraw || _raw_data_prev[1] != yraw || _raw_data_prev[2] != zraw) {
+        _px4_mag.set_error_count(perf_event_count(_bad_transfer_perf)
+                                 + perf_event_count(_range_error_perf));
+
+        _px4_mag.update(timestamp_sample, xraw, yraw, zraw);
+
+        _raw_data_prev[0] = xraw;
+        _raw_data_prev[1] = yraw;
+        _raw_data_prev[2] = zraw;
+
+        if (_failure_count > 0) {
+            _failure_count--;
+        }
+
+    } else {
+        _failure_count++;
+    }
 }
 
-void RM3100::convert_signed(int32_t *n)
-{
-	/* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */
-	if ((*n & (1 << 23)) == (1 << 23)) {
-		*n |= 0xFF000000;
-	}
+void RM3100::convert_signed(int32_t *n) {
+    /* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */
+    if ((*n & (1 << 23)) == (1 << 23)) {
+        *n |= 0xFF000000;
+    }
 }
 
-int RM3100::init()
-{
-	int ret = self_test();
+int RM3100::init() {
+    int ret = self_test();
 
-	if (ret != PX4_OK) {
-		PX4_ERR("self test failed");
-	}
+    if (ret != PX4_OK) {
+        PX4_ERR("self test failed");
+    }
 
-	if (set_default_register_values() == PX4_OK) {
-		ScheduleOnInterval(RM3100_INTERVAL);
-		return PX4_OK;
-	}
+    if (set_default_register_values() == PX4_OK) {
+        ScheduleOnInterval(RM3100_INTERVAL);
+        return PX4_OK;
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void RM3100::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_range_error_perf);
-	perf_print_counter(_bad_transfer_perf);
+void RM3100::print_status() {
+    I2CSPIDriverBase::print_status();
+    perf_print_counter(_reset_perf);
+    perf_print_counter(_range_error_perf);
+    perf_print_counter(_bad_transfer_perf);
 }
 
-int RM3100::set_default_register_values()
-{
-	perf_count(_reset_perf);
+int RM3100::set_default_register_values() {
+    perf_count(_reset_perf);
 
-	uint8_t cmd[2] = {0, 0};
+    uint8_t cmd[2] = {0, 0};
 
-	cmd[0] = CCX_DEFAULT_MSB;
-	cmd[1] = CCX_DEFAULT_LSB;
-	_interface->write(ADDR_CCX, cmd, 2);
+    cmd[0] = CCX_DEFAULT_MSB;
+    cmd[1] = CCX_DEFAULT_LSB;
+    _interface->write(ADDR_CCX, cmd, 2);
 
-	cmd[0] = CCY_DEFAULT_MSB;
-	cmd[1] = CCY_DEFAULT_LSB;
-	_interface->write(ADDR_CCY, cmd, 2);
+    cmd[0] = CCY_DEFAULT_MSB;
+    cmd[1] = CCY_DEFAULT_LSB;
+    _interface->write(ADDR_CCY, cmd, 2);
 
-	cmd[0] = CCZ_DEFAULT_MSB;
-	cmd[1] = CCZ_DEFAULT_LSB;
-	_interface->write(ADDR_CCZ, cmd, 2);
+    cmd[0] = CCZ_DEFAULT_MSB;
+    cmd[1] = CCZ_DEFAULT_LSB;
+    _interface->write(ADDR_CCZ, cmd, 2);
 
-	cmd[0] = HSHAKE_DEFAULT;
-	_interface->write(ADDR_HSHAKE, cmd, 1);
+    cmd[0] = HSHAKE_DEFAULT;
+    _interface->write(ADDR_HSHAKE, cmd, 1);
 
-	cmd[0] = CMM_DEFAULT;
-	_interface->write(ADDR_CMM, cmd, 1);
+    cmd[0] = CMM_DEFAULT;
+    _interface->write(ADDR_CMM, cmd, 1);
 
-	cmd[0] = TMRC_DEFAULT;
-	_interface->write(ADDR_TMRC, cmd, 1);
+    cmd[0] = TMRC_DEFAULT;
+    _interface->write(ADDR_TMRC, cmd, 1);
 
-	cmd[0] = BIST_DEFAULT;
-	_interface->write(ADDR_BIST, cmd, 1);
+    cmd[0] = BIST_DEFAULT;
+    _interface->write(ADDR_BIST, cmd, 1);
 
-	return PX4_OK;
+    return PX4_OK;
 }
diff --git a/apps/peripheral/sensor/mag/rm3100/rm3100.h b/apps/peripheral/sensor/mag/rm3100/rm3100.h
index 7e4737d48b..9e014fada9 100644
--- a/apps/peripheral/sensor/mag/rm3100/rm3100.h
+++ b/apps/peripheral/sensor/mag/rm3100/rm3100.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rm3100.h
@@ -50,95 +27,93 @@
  * RM3100 internal constants and data structures.
  */
 
-#define RM3100_INTERVAL			13000	// 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95)
-#define UTESLA_TO_GAUSS			100.0f
-#define RM3100_SENSITIVITY		75.0f
-
-#define ADDR_POLL		0x00
-#define ADDR_CMM		0x01
-#define ADDR_CCX		0x04
-#define ADDR_CCY		0x06
-#define ADDR_CCZ		0x08
-#define ADDR_TMRC		0x0B
-#define ADDR_MX			0x24
-#define ADDR_MY			0x27
-#define ADDR_MZ			0x2A
-#define ADDR_BIST		0x33
-#define ADDR_STATUS		0x34
-#define ADDR_HSHAKE		0x35
-#define ADDR_REVID		0x36
-
-#define CCX_DEFAULT_MSB		0x00
-#define CCX_DEFAULT_LSB		0xC8
-#define CCY_DEFAULT_MSB		CCX_DEFAULT_MSB
-#define CCY_DEFAULT_LSB		CCX_DEFAULT_LSB
-#define CCZ_DEFAULT_MSB		CCX_DEFAULT_MSB
-#define CCZ_DEFAULT_LSB		CCX_DEFAULT_LSB
-#define CMM_DEFAULT		0b0111'0001 // continuous mode
-#define CONTINUOUS_MODE		(1 << 0)
-#define POLLING_MODE		(0 << 0)
-#define TMRC_DEFAULT		0x95 // ~13 ms, ~75 Hz
-#define BIST_SELFTEST		0b1000'1111
-#define BIST_DEFAULT		0x00
-#define BIST_XYZ_OK		((1 << 4) | (1 << 5) | (1 << 6))
-#define BIST_STE		(1 << 7)
-#define BIST_DUR_USEC		(2*RM3100_INTERVAL)
-#define HSHAKE_DEFAULT		(0x0B)
-#define HSHAKE_NO_DRDY_CLEAR	(0x08)
-#define STATUS_DRDY		(1 << 7)
-#define POLL_XYZ		0x70
-
-#define RM3100_REVID		0x22
+#define RM3100_INTERVAL      13000 // 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95)
+#define UTESLA_TO_GAUSS      100.0f
+#define RM3100_SENSITIVITY   75.0f
+
+#define ADDR_POLL            0x00
+#define ADDR_CMM             0x01
+#define ADDR_CCX             0x04
+#define ADDR_CCY             0x06
+#define ADDR_CCZ             0x08
+#define ADDR_TMRC            0x0B
+#define ADDR_MX              0x24
+#define ADDR_MY              0x27
+#define ADDR_MZ              0x2A
+#define ADDR_BIST            0x33
+#define ADDR_STATUS          0x34
+#define ADDR_HSHAKE          0x35
+#define ADDR_REVID           0x36
+
+#define CCX_DEFAULT_MSB      0x00
+#define CCX_DEFAULT_LSB      0xC8
+#define CCY_DEFAULT_MSB      CCX_DEFAULT_MSB
+#define CCY_DEFAULT_LSB      CCX_DEFAULT_LSB
+#define CCZ_DEFAULT_MSB      CCX_DEFAULT_MSB
+#define CCZ_DEFAULT_LSB      CCX_DEFAULT_LSB
+#define CMM_DEFAULT          0b0111'0001 // continuous mode
+#define CONTINUOUS_MODE      (1 << 0)
+#define POLLING_MODE         (0 << 0)
+#define TMRC_DEFAULT         0x95 // ~13 ms, ~75 Hz
+#define BIST_SELFTEST        0b1000'1111
+#define BIST_DEFAULT         0x00
+#define BIST_XYZ_OK          ((1 << 4) | (1 << 5) | (1 << 6))
+#define BIST_STE             (1 << 7)
+#define BIST_DUR_USEC        (2 * RM3100_INTERVAL)
+#define HSHAKE_DEFAULT       (0x0B)
+#define HSHAKE_NO_DRDY_CLEAR (0x08)
+#define STATUS_DRDY          (1 << 7)
+#define POLL_XYZ             0x70
+
+#define RM3100_REVID         0x22
 
 /* interface factories */
 extern device::Device *RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
 extern device::Device *RM3100_I2C_interface(int bus, int bus_frequency);
 
-#define RM3100_ADDRESS		0x20
+#define RM3100_ADDRESS 0x20
 
-class RM3100 : public I2CSPIDriver<RM3100>
-{
+class RM3100 : public I2CSPIDriver<RM3100> {
 public:
-	RM3100(device::Device *interface, const I2CSPIDriverConfig &config);
-	virtual ~RM3100();
+    RM3100(device::Device *interface, const I2CSPIDriverConfig &config);
+    virtual ~RM3100();
 
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
+    static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+    static void              print_usage();
 
-	int init();
+    int init();
 
-	void print_status() override;
+    void print_status() override;
 
-	/**
+    /**
 	 * Configures the device with default register values.
 	 */
-	int set_default_register_values();
+    int set_default_register_values();
 
-	void RunImpl();
+    void RunImpl();
 
 private:
-	/**
+    /**
 	 * Run sensor self-test
 	 *
 	 * @return 0 if self-test is ok, 1 else
 	 */
-	int self_test();
+    int self_test();
 
-	/**
+    /**
 	* Converts int24_t stored in 32-bit container to int32_t
 	*/
-	void convert_signed(int32_t *n);
-
-	PX4Magnetometer _px4_mag;
+    void convert_signed(int32_t *n);
 
-	device::Device *_interface{nullptr};
+    PX4Magnetometer _px4_mag;
 
-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _range_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": range error")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+    device::Device *_interface{nullptr};
 
-	int32_t _raw_data_prev[3] {};
+    perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")};
+    perf_counter_t _range_error_perf{perf_alloc(PC_COUNT, MODULE_NAME ": range error")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
 
-	int _failure_count{0};
+    int32_t _raw_data_prev[3]{};
 
+    int _failure_count{0};
 };
diff --git a/apps/peripheral/sensor/mag/rm3100/rm3100_i2c.cpp b/apps/peripheral/sensor/mag/rm3100/rm3100_i2c.cpp
index 3aeba5d57e..219af6247c 100644
--- a/apps/peripheral/sensor/mag/rm3100/rm3100_i2c.cpp
+++ b/apps/peripheral/sensor/mag/rm3100/rm3100_i2c.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rm3100_i2c.cpp
@@ -52,80 +29,74 @@
 #include "board_config.h"
 #include "rm3100.h"
 
-class RM3100_I2C : public device::I2C
-{
+class RM3100_I2C : public device::I2C {
 public:
-	RM3100_I2C(int bus, int bus_frequency);
-	~RM3100_I2C() override = default;
+    RM3100_I2C(int bus, int bus_frequency);
+    ~RM3100_I2C() override = default;
 
-	int     read(unsigned address, void *data, unsigned count) override;
-	int     write(unsigned address, void *data, unsigned count) override;
+    int read(unsigned address, void *data, unsigned count) override;
+    int write(unsigned address, void *data, unsigned count) override;
 
 protected:
-	int     probe() override;
+    int probe() override;
 };
 
 device::Device *
 RM3100_I2C_interface(int bus, int bus_frequency);
 
 device::Device *
-RM3100_I2C_interface(int bus, int bus_frequency)
-{
-	return new RM3100_I2C(bus, bus_frequency);
+RM3100_I2C_interface(int bus, int bus_frequency) {
+    return new RM3100_I2C(bus, bus_frequency);
 }
 
 RM3100_I2C::RM3100_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_RM3100, MODULE_NAME, bus, RM3100_ADDRESS, bus_frequency)
-{
+    I2C(DRV_MAG_DEVTYPE_RM3100, MODULE_NAME, bus, RM3100_ADDRESS, bus_frequency) {
 }
 
-int RM3100_I2C::probe()
-{
-	uint8_t data = 0;
+int RM3100_I2C::probe() {
+    uint8_t data = 0;
 
-	if (read(ADDR_REVID, &data, 1)) {
-		DEVICE_DEBUG("RM3100 read_reg fail");
-		return -EIO;
-	}
+    if (read(ADDR_REVID, &data, 1)) {
+        DEVICE_DEBUG("RM3100 read_reg fail");
+        return -EIO;
+    }
 
-	if (data != RM3100_REVID) {
-		DEVICE_DEBUG("RM3100 bad ID: %02x", data);
-		return -EIO;
-	}
+    if (data != RM3100_REVID) {
+        DEVICE_DEBUG("RM3100 bad ID: %02x", data);
+        return -EIO;
+    }
 
-	_retries = 1;
+    _retries = 1;
 
-	return OK;
+    return OK;
 }
 
-int RM3100_I2C::read(unsigned address, void *data, unsigned count)
-{
-	uint8_t cmd = address;
-	int ret;
+int RM3100_I2C::read(unsigned address, void *data, unsigned count) {
+    uint8_t cmd = address;
+    int     ret;
 
-	/* We need a first transfer where we write the register to read */
-	ret = transfer(&cmd, 1, nullptr, 0);
+    /* We need a first transfer where we write the register to read */
+    ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (ret != OK) {
-		return ret;
-	}
+    if (ret != OK) {
+        return ret;
+    }
 
-	/* Now we read the previously selected register */
-	ret = transfer(nullptr, 0, (uint8_t *)data, count);
+    /* Now we read the previously selected register */
+    ret = transfer(nullptr, 0, (uint8_t *)data, count);
 
-	return ret;
+    return ret;
 }
 
-int RM3100_I2C::write(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
+int RM3100_I2C::write(unsigned address, void *data, unsigned count) {
+    uint8_t buf[32];
 
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
+    if (sizeof(buf) < (count + 1)) {
+        return -EIO;
+    }
 
-	buf[0] = address;
-	memcpy(&buf[1], data, count);
+    buf[0] = address;
+    memcpy(&buf[1], data, count);
 
-	return transfer(&buf[0], count + 1, nullptr, 0);
+    return transfer(&buf[0], count + 1, nullptr, 0);
 }
diff --git a/apps/peripheral/sensor/mag/rm3100/rm3100_main.cpp b/apps/peripheral/sensor/mag/rm3100/rm3100_main.cpp
index be8571cced..6e87066226 100644
--- a/apps/peripheral/sensor/mag/rm3100/rm3100_main.cpp
+++ b/apps/peripheral/sensor/mag/rm3100/rm3100_main.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rm3100_main.cpp
@@ -41,90 +18,87 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-I2CSPIDriverBase *RM3100::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	device::Device *interface = nullptr;
+I2CSPIDriverBase *RM3100::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) {
+    device::Device *interface = nullptr;
 
-	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = RM3100_I2C_interface(config.bus, config.bus_frequency);
+    if (config.bus_type == BOARD_I2C_BUS) {
+        interface = RM3100_I2C_interface(config.bus, config.bus_frequency);
 
-	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = RM3100_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
-	}
+    } else if (config.bus_type == BOARD_SPI_BUS) {
+        interface = RM3100_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+    }
 
-	if (interface == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
+    if (interface == nullptr) {
+        PX4_ERR("alloc failed");
+        return nullptr;
+    }
 
-	if (interface->init() != OK) {
-		delete interface;
-		PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid);
-		return nullptr;
-	}
+    if (interface->init() != OK) {
+        delete interface;
+        PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid);
+        return nullptr;
+    }
 
-	RM3100 *dev = new RM3100(interface, config);
+    RM3100 *dev = new RM3100(interface, config);
 
-	if (dev == nullptr) {
-		delete interface;
-		return nullptr;
-	}
+    if (dev == nullptr) {
+        delete interface;
+        return nullptr;
+    }
 
-	if (OK != dev->init()) {
-		delete dev;
-		return nullptr;
-	}
+    if (OK != dev->init()) {
+        delete dev;
+        return nullptr;
+    }
 
-	return dev;
+    return dev;
 }
 
-void RM3100::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("rm3100", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void RM3100::print_usage() {
+    PRINT_MODULE_USAGE_NAME("rm3100", "driver");
+    PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" int rm3100_main(int argc, char *argv[])
-{
-	using ThisDriver = RM3100;
-	int ch;
-	BusCLIArguments cli{true, true};
-	cli.default_i2c_frequency = 400000;
-	cli.default_spi_frequency = 1 * 1000 * 1000;
+extern "C" int rm3100_main(int argc, char *argv[]) {
+    using ThisDriver = RM3100;
+    int             ch;
+    BusCLIArguments cli{true, true};
+    cli.default_i2c_frequency = 400000;
+    cli.default_spi_frequency = 1 * 1000 * 1000;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+        switch (ch) {
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	cli.i2c_address = RM3100_ADDRESS;
+    cli.i2c_address = RM3100_ADDRESS;
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_RM3100);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_RM3100);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/mag/rm3100/rm3100_spi.cpp b/apps/peripheral/sensor/mag/rm3100/rm3100_spi.cpp
index 3d8f84498a..64a13414b2 100644
--- a/apps/peripheral/sensor/mag/rm3100/rm3100_spi.cpp
+++ b/apps/peripheral/sensor/mag/rm3100/rm3100_spi.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file rm3100_spi.cpp
@@ -53,85 +30,79 @@
 #include "rm3100.h"
 
 /* SPI protocol address bits */
-#define DIR_READ        (1<<7)
-#define DIR_WRITE       (0<<7)
+#define DIR_READ  (1 << 7)
+#define DIR_WRITE (0 << 7)
 
-class RM3100_SPI : public device::SPI
-{
+class RM3100_SPI : public device::SPI {
 public:
-	RM3100_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-	virtual ~RM3100_SPI() = default;
+    RM3100_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+    virtual ~RM3100_SPI() = default;
 
-	virtual int     init();
-	virtual int     read(unsigned address, void *data, unsigned count);
-	virtual int     write(unsigned address, void *data, unsigned count);
+    virtual int init();
+    virtual int read(unsigned address, void *data, unsigned count);
+    virtual int write(unsigned address, void *data, unsigned count);
 };
 
 device::Device *
 RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
 
 device::Device *
-RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
-{
-	return new RM3100_SPI(bus, devid, bus_frequency, spi_mode);
+RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) {
+    return new RM3100_SPI(bus, devid, bus_frequency, spi_mode);
 }
 
 RM3100_SPI::RM3100_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_RM3100, MODULE_NAME, bus, devid, spi_mode, bus_frequency)
-{
+    SPI(DRV_MAG_DEVTYPE_RM3100, MODULE_NAME, bus, devid, spi_mode, bus_frequency) {
 }
 
-int RM3100_SPI::init()
-{
-	int ret;
+int RM3100_SPI::init() {
+    int ret;
 
-	ret = SPI::init();
+    ret = SPI::init();
 
-	if (ret != OK) {
-		DEVICE_DEBUG("SPI init failed");
-		return -EIO;
-	}
+    if (ret != OK) {
+        DEVICE_DEBUG("SPI init failed");
+        return -EIO;
+    }
 
-	// Read REV_ID value
-	uint8_t data = 0;
+    // Read REV_ID value
+    uint8_t data = 0;
 
-	if (read(ADDR_REVID, &data, 1)) {
-		DEVICE_DEBUG("RM3100 read_reg fail");
-	}
+    if (read(ADDR_REVID, &data, 1)) {
+        DEVICE_DEBUG("RM3100 read_reg fail");
+    }
 
-	if (data != RM3100_REVID) {
-		DEVICE_DEBUG("RM3100 ID: %02x", data);
-		return -EIO;
-	}
+    if (data != RM3100_REVID) {
+        DEVICE_DEBUG("RM3100 ID: %02x", data);
+        return -EIO;
+    }
 
-	return OK;
+    return OK;
 }
 
-int RM3100_SPI::read(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
+int RM3100_SPI::read(unsigned address, void *data, unsigned count) {
+    uint8_t buf[32];
 
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
+    if (sizeof(buf) < (count + 1)) {
+        return -EIO;
+    }
 
-	buf[0] = address | DIR_READ;
+    buf[0] = address | DIR_READ;
 
-	int ret = transfer(&buf[0], &buf[0], count + 1);
-	memcpy(data, &buf[1], count);
-	return ret;
+    int ret = transfer(&buf[0], &buf[0], count + 1);
+    memcpy(data, &buf[1], count);
+    return ret;
 }
 
-int RM3100_SPI::write(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
+int RM3100_SPI::write(unsigned address, void *data, unsigned count) {
+    uint8_t buf[32];
 
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
+    if (sizeof(buf) < (count + 1)) {
+        return -EIO;
+    }
 
-	buf[0] = address | DIR_WRITE;
-	memcpy(&buf[1], data, count);
+    buf[0] = address | DIR_WRITE;
+    memcpy(&buf[1], data, count);
 
-	return transfer(&buf[0], &buf[0], count + 1);
+    return transfer(&buf[0], &buf[0], count + 1);
 }
diff --git a/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.cpp b/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.cpp
index f5e5facd1e..92df4583f2 100644
--- a/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.cpp
+++ b/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.cpp
@@ -1,744 +1,698 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2022-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PAA3905.hpp"
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
 }
 
 PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
-	SPI(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");
-	}
+    SPI(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;
+    float yaw_rotation_degrees = (float)config.custom1;
 
-	if (yaw_rotation_degrees >= 0.f) {
-		PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
-			 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
+    if (yaw_rotation_degrees >= 0.f) {
+        PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
+                 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
 
-		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
+        _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
 
-	} else {
-		_rotation.identity();
-	}
+    } else {
+        _rotation.identity();
+    }
 }
 
-PAA3905::~PAA3905()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_reset_perf);
-	perf_free(_false_motion_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);
+PAA3905::~PAA3905() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_reset_perf);
+    perf_free(_false_motion_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()
-{
-	int ret = SPI::init();
+int PAA3905::init() {
+    int ret = SPI::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("SPI::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool PAA3905::Reset()
-{
-	_state = STATE::RESET;
-	DataReadyInterruptDisable();
-	_drdy_timestamp_sample.store(0);
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool PAA3905::Reset() {
+    _state = STATE::RESET;
+    DataReadyInterruptDisable();
+    _drdy_timestamp_sample.store(0);
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
 
-void PAA3905::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+void PAA3905::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void PAA3905::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_false_motion_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);
+void PAA3905::print_status() {
+    I2CSPIDriverBase::print_status();
+
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_reset_perf);
+    perf_print_counter(_false_motion_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);
 }
 
-int PAA3905::probe()
-{
-	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);
+int PAA3905::probe() {
+    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) {
-			DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
-			break;
-		}
+        if (Product_ID != PRODUCT_ID) {
+            DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
+            break;
+        }
 
-		if (Revision_ID != REVISION_ID) {
-			DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
-			break;
-		}
+        if (Revision_ID != REVISION_ID) {
+            DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
+            break;
+        }
 
-		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
-			DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
-			break;
-		}
+        if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+            DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
+            break;
+        }
 
-		return PX4_OK;
-	}
+        return PX4_OK;
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void PAA3905::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// Issue a soft reset
-		RegisterWrite(Register::Power_Up_Reset, 0x5A);
-
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		perf_count(_reset_perf);
-		ScheduleDelayed(1_ms);
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
-			// 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);
-
-			_discard_reading = 3;
-
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleNow();
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 100 ms");
-				ScheduleDelayed(100_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start measurement cycle
-			_state = STATE::READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_motion_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(1_s);
-
-			} else {
-				_motion_interrupt_enabled = false;
-				ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
-			}
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::READ: {
-			hrt_abstime timestamp_sample = now;
-
-			if (_motion_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(kBackupScheduleIntervalUs);
-			}
-
-			struct TransferBuffer {
-				uint8_t cmd = Register::Motion_Burst;
-				BURST_TRANSFER data{};
-			} buffer{};
-			static_assert(sizeof(buffer) == (14 + 1));
-
-			bool success = false;
-
-			if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
-
-				hrt_store_absolute_time(&_last_read_time);
-
-				if (_discard_reading > 0) {
-					_discard_reading--;
-				}
-
-				if (buffer.data.RawData_Sum > 0x98) {
-					perf_count(_bad_register_perf);
-					PX4_ERR("invalid RawData_Sum > 0x98");
-				}
-
-				// Bit [5:0] check if chip is working correctly
-				//  0x3F: chip is working correctly
-				if ((buffer.data.Observation & 0x3F) != 0x3F) {
-					// Other value: recommend to issue a software reset
-					perf_count(_bad_register_perf);
-					PX4_ERR("Observation not equal to 0x3F, resetting");
-					Reset();
-					return;
-
-				} else {
-					// Observation: check mode
-					const Mode prev_mode = _mode;
-
-					// Bit [7:6] AMS mode
-					const uint8_t ams_mode = (buffer.data.Observation & (Bit7 | Bit6)) >> 6;
-
-					if (ams_mode == 0x0) {
-						// Mode 0 (Bright)
-						if (_mode != Mode::Bright) {
-							_mode = Mode::Bright;
-							_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
-							perf_count(_mode_change_bright_perf);
-						}
-
-					} else if (ams_mode == 0x1) {
-						// Mode 1 (LowLight)
-						if (_mode != Mode::LowLight) {
-							_mode = Mode::LowLight;
-							_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
-							perf_count(_mode_change_low_light_perf);
-						}
-
-					} else if (ams_mode == 0x2) {
-						// Mode 2 (SuperLowLight)
-						if (_mode != Mode::SuperLowLight) {
-							_mode = Mode::SuperLowLight;
-							_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
-							perf_count(_mode_change_super_low_light_perf);
-						}
-
-					} else {
-						perf_count(_bad_register_perf);
-						PX4_ERR("invalid mode (%d) Observation: %X", ams_mode, buffer.data.Observation);
-						Reset();
-						return;
-					}
-
-					if (prev_mode != _mode) {
-						// update scheduling on mode change
-						if (!_motion_interrupt_enabled) {
-							ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
-						}
-					}
-				}
-
-				if (buffer.data.Motion & Motion_Bit::ChallengingSurface) {
-					PX4_WARN("challenging surface detected");
-				}
-
-				// publish sensor_optical_flow
-				sensor_optical_flow_s sensor_optical_flow{};
-				sensor_optical_flow.timestamp_sample = timestamp_sample;
-				sensor_optical_flow.device_id = get_device_id();
-
-				sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
-
-				// set specs according to datasheet
-				sensor_optical_flow.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
-				sensor_optical_flow.min_ground_distance = 0.08f;    // Datasheet: 80mm
-				sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
-
-				// 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
-
-				// 23-bit Shutter register
-				const uint8_t shutter_lower = buffer.data.Shutter_Lower;
-				const uint8_t shutter_middle = buffer.data.Shutter_Middle;
-				const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
-
-				const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
-
-				// Number of Features = SQUAL * 4
-				// RawData_Sum maximum register value is 0x98
-				bool data_valid = (buffer.data.SQUAL > 0)
-						  && (buffer.data.RawData_Sum > 0)
-						  && (buffer.data.RawData_Sum <= 0x98)
-						  && (shutter > 0)
-						  && (_discard_reading == 0);
-
-				switch (_mode) {
-				case Mode::Bright:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_BRIGHT;
-
-					// quality < 25 (0x19) and shutter >= 0x00FF80
-					if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					break;
-
-				case Mode::LowLight:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
-
-					// quality < 70 (0x46) and shutter >= 0x00FF80
-					if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					break;
-
-				case Mode::SuperLowLight:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
-
-					// quality < 85 (0x55) and shutter >= 0x025998
-					if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x025998)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					break;
-				}
-
-				// motion in burst transfer
-				const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
-
-				const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
-				const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
-
-				if (data_valid) {
-
-					const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
-					const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
-
-					bool publish = false;
-
-					if (motion_reported) {
-						// 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);
+void PAA3905::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
 
-						sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
-						sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+    switch (_state) {
+    case STATE::RESET:
+        // Issue a soft reset
+        RegisterWrite(Register::Power_Up_Reset, 0x5A);
 
-						sensor_optical_flow.quality = buffer.data.SQUAL;
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        perf_count(_reset_perf);
+        ScheduleDelayed(1_ms);
+        break;
 
-						publish = true;
+    case STATE::WAIT_FOR_RESET:
+        if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
+            // 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);
+
+            _discard_reading = 3;
+
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleNow();
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
+
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 100 ms");
+                ScheduleDelayed(100_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start measurement cycle
+            _state = STATE::READ;
+
+            if (DataReadyInterruptConfigure()) {
+                _motion_interrupt_enabled = true;
+
+                // backup schedule as a watchdog timeout
+                ScheduleDelayed(1_s);
+
+            } else {
+                _motion_interrupt_enabled = false;
+                ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+            }
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::READ: {
+        hrt_abstime timestamp_sample = now;
+
+        if (_motion_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(kBackupScheduleIntervalUs);
+        }
+
+        struct TransferBuffer {
+            uint8_t        cmd = Register::Motion_Burst;
+            BURST_TRANSFER data{};
+        } buffer{};
+
+        static_assert(sizeof(buffer) == (14 + 1));
+
+        bool success = false;
+
+        if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
+            hrt_store_absolute_time(&_last_read_time);
+
+            if (_discard_reading > 0) {
+                _discard_reading--;
+            }
+
+            if (buffer.data.RawData_Sum > 0x98) {
+                perf_count(_bad_register_perf);
+                PX4_ERR("invalid RawData_Sum > 0x98");
+            }
+
+            // Bit [5:0] check if chip is working correctly
+            //  0x3F: chip is working correctly
+            if ((buffer.data.Observation & 0x3F) != 0x3F) {
+                // Other value: recommend to issue a software reset
+                perf_count(_bad_register_perf);
+                PX4_ERR("Observation not equal to 0x3F, resetting");
+                Reset();
+                return;
+
+            } else {
+                // Observation: check mode
+                const Mode prev_mode = _mode;
+
+                // Bit [7:6] AMS mode
+                const uint8_t ams_mode = (buffer.data.Observation & (Bit7 | Bit6)) >> 6;
+
+                if (ams_mode == 0x0) {
+                    // Mode 0 (Bright)
+                    if (_mode != Mode::Bright) {
+                        _mode                  = Mode::Bright;
+                        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
+                        perf_count(_mode_change_bright_perf);
+                    }
+
+                } else if (ams_mode == 0x1) {
+                    // Mode 1 (LowLight)
+                    if (_mode != Mode::LowLight) {
+                        _mode                  = Mode::LowLight;
+                        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
+                        perf_count(_mode_change_low_light_perf);
+                    }
+
+                } else if (ams_mode == 0x2) {
+                    // Mode 2 (SuperLowLight)
+                    if (_mode != Mode::SuperLowLight) {
+                        _mode                  = Mode::SuperLowLight;
+                        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
+                        perf_count(_mode_change_super_low_light_perf);
+                    }
+
+                } else {
+                    perf_count(_bad_register_perf);
+                    PX4_ERR("invalid mode (%d) Observation: %X", ams_mode, buffer.data.Observation);
+                    Reset();
+                    return;
+                }
+
+                if (prev_mode != _mode) {
+                    // update scheduling on mode change
+                    if (!_motion_interrupt_enabled) {
+                        ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+                    }
+                }
+            }
+
+            if (buffer.data.Motion & Motion_Bit::ChallengingSurface) {
+                PX4_WARN("challenging surface detected");
+            }
+
+            // publish sensor_optical_flow
+            sensor_optical_flow_s sensor_optical_flow{};
+            sensor_optical_flow.timestamp_sample = timestamp_sample;
+            sensor_optical_flow.device_id        = get_device_id();
+
+            sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
+
+            // set specs according to datasheet
+            sensor_optical_flow.max_flow_rate       = 7.4f;     // Datasheet: 7.4 rad/s
+            sensor_optical_flow.min_ground_distance = 0.08f;    // Datasheet: 80mm
+            sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
+
+            // 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
+
+            // 23-bit Shutter register
+            const uint8_t shutter_lower  = buffer.data.Shutter_Lower;
+            const uint8_t shutter_middle = buffer.data.Shutter_Middle;
+            const uint8_t shutter_upper  = buffer.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
+
+            const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
+
+            // Number of Features = SQUAL * 4
+            // RawData_Sum maximum register value is 0x98
+            bool data_valid = (buffer.data.SQUAL > 0)
+                           && (buffer.data.RawData_Sum > 0)
+                           && (buffer.data.RawData_Sum <= 0x98)
+                           && (shutter > 0)
+                           && (_discard_reading == 0);
+
+            switch (_mode) {
+            case Mode::Bright:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_BRIGHT;
+
+                // quality < 25 (0x19) and shutter >= 0x00FF80
+                if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                break;
+
+            case Mode::LowLight:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_LOWLIGHT;
+
+                // quality < 70 (0x46) and shutter >= 0x00FF80
+                if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                break;
+
+            case Mode::SuperLowLight:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
+
+                // quality < 85 (0x55) and shutter >= 0x025998
+                if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x025998)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                break;
+            }
+
+            // motion in burst transfer
+            const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
+
+            const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
+            const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
+
+            if (data_valid) {
+                const bool zero_flow         = (delta_x_raw == 0) && (delta_y_raw == 0);
+                const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
+
+                bool publish = false;
+
+                if (motion_reported) {
+                    // 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);
+
+                    sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
+                    sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+
+                    sensor_optical_flow.quality = buffer.data.SQUAL;
+
+                    publish = true;
+
+                    _last_motion = timestamp_sample;
+
+                } else if (zero_flow && (timestamp_sample > _last_motion)) {
+                    // no motion, but burst read looks valid and we should have seen new data by now if there was any motion
+                    const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
+                                                 || (shutter != _shutter_prev)
+                                                 || (buffer.data.RawData_Sum != _raw_data_sum_prev)
+                                                 || (buffer.data.SQUAL != _quality_prev);
+
+                    if (burst_read_changed) {
+                        sensor_optical_flow.pixel_flow[0] = 0;
+                        sensor_optical_flow.pixel_flow[1] = 0;
+
+                        sensor_optical_flow.quality = buffer.data.SQUAL;
+
+                        publish = true;
+                    }
+                }
 
-						_last_motion = timestamp_sample;
+                // only publish when there's valid data or on timeout
+                if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+                    sensor_optical_flow.timestamp = hrt_absolute_time();
+                    _sensor_optical_flow_pub.publish(sensor_optical_flow);
+
+                    _last_publish = sensor_optical_flow.timestamp_sample;
+                }
 
-					} else if (zero_flow && (timestamp_sample > _last_motion)) {
-						// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
-						const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
-										|| (shutter != _shutter_prev)
-										|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
-										|| (buffer.data.SQUAL != _quality_prev);
+                // backup schedule if we're reliant on the motion interrupt and there's very little flow
+                if (_motion_interrupt_enabled && little_to_no_flow) {
+                    switch (_mode) {
+                    case Mode::Bright:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
+                        break;
 
-						if (burst_read_changed) {
+                    case Mode::LowLight:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
+                        break;
 
-							sensor_optical_flow.pixel_flow[0] = 0;
-							sensor_optical_flow.pixel_flow[1] = 0;
+                    case Mode::SuperLowLight:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
+                        break;
+                    }
+                }
 
-							sensor_optical_flow.quality = buffer.data.SQUAL;
+                success = true;
 
-							publish = true;
-						}
-					}
+                if (_failure_count > 0) {
+                    _failure_count--;
+                }
+            }
 
-					// only publish when there's valid data or on timeout
-					if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+            _delta_x_raw_prev  = delta_x_raw;
+            _delta_y_raw_prev  = delta_y_raw;
+            _shutter_prev      = shutter;
+            _raw_data_sum_prev = buffer.data.RawData_Sum;
+            _quality_prev      = buffer.data.SQUAL;
 
-						sensor_optical_flow.timestamp = hrt_absolute_time();
-						_sensor_optical_flow_pub.publish(sensor_optical_flow);
+        } else {
+            perf_count(_bad_transfer_perf);
+        }
 
-						_last_publish = sensor_optical_flow.timestamp_sample;
-					}
+        if (!success) {
+            _failure_count++;
 
-					// backup schedule if we're reliant on the motion interrupt and there's very little flow
-					if (_motion_interrupt_enabled && little_to_no_flow) {
-						switch (_mode) {
-						case Mode::Bright:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
-							break;
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+            }
+        }
+    }
 
-						case Mode::LowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
-							break;
-
-						case Mode::SuperLowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
-							break;
-						}
-					}
-
-					success = true;
-
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
-
-				_delta_x_raw_prev = delta_x_raw;
-				_delta_y_raw_prev = delta_y_raw;
-				_shutter_prev = shutter;
-				_raw_data_sum_prev = buffer.data.RawData_Sum;
-				_quality_prev = buffer.data.SQUAL;
-
-			} else {
-				perf_count(_bad_transfer_perf);
-			}
-
-			if (!success) {
-				_failure_count++;
-
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-				}
-			}
-		}
-
-		break;
-	}
+    break;
+    }
 }
 
-bool PAA3905::Configure()
-{
-	ConfigureStandardDetectionSetting();
-	// ConfigureEnhancedDetectionMode();
+bool PAA3905::Configure() {
+    ConfigureStandardDetectionSetting();
+    // ConfigureEnhancedDetectionMode();
 
-	ConfigureAutomaticModeSwitching();
+    ConfigureAutomaticModeSwitching();
 
-	EnableLed();
+    EnableLed();
 
-	return true;
+    return true;
 }
 
-void PAA3905::ConfigureStandardDetectionSetting()
-{
-	// 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.
-
-	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);
-
-	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::ConfigureStandardDetectionSetting() {
+    // 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.
+
+    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);
+
+    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::ConfigureEnhancedDetectionMode()
-{
-	// 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.
-
-	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::ConfigureEnhancedDetectionMode() {
+    // 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.
+
+    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::ConfigureAutomaticModeSwitching()
-{
-	// Automatic switching between Mode 0, 1 and 2:
-	RegisterWrite(0x7F, 0x08);
-	RegisterWrite(0x68, 0x02);
-	RegisterWrite(0x7F, 0x00);
-
-	// 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::ConfigureAutomaticModeSwitching() {
+    // Automatic switching between Mode 0, 1 and 2:
+    RegisterWrite(0x7F, 0x08);
+    RegisterWrite(0x68, 0x02);
+    RegisterWrite(0x7F, 0x00);
+
+    // 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
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x6F, 0x0C);
-	RegisterWrite(0x7F, 0x00);
+void PAA3905::EnableLed() {
+    // Enable LED_N controls
+    RegisterWrite(0x7F, 0x14);
+    RegisterWrite(0x6F, 0x0C);
+    RegisterWrite(0x7F, 0x00);
 }
 
-int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<PAA3905 *>(arg)->DataReady();
-	return 0;
+int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<PAA3905 *>(arg)->DataReady();
+    return 0;
 }
 
-void PAA3905::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void PAA3905::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool PAA3905::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool PAA3905::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
+    // Setup data ready on falling edge
+    return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
-bool PAA3905::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool PAA3905::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
+    return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
-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);
+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);
-	}
+    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);
+    // 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);
-	}
+    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);
+    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];
+    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);
+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);
-	}
+    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;
-	transfer(&cmd[0], nullptr, sizeof(cmd));
-	hrt_store_absolute_time(&_last_write_time);
+    uint8_t cmd[2];
+    cmd[0] = DIR_WRITE(reg);
+    cmd[1] = data;
+    transfer(&cmd[0], nullptr, sizeof(cmd));
+    hrt_store_absolute_time(&_last_write_time);
 }
diff --git a/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.hpp b/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.hpp
index c850b0405a..0d67bbaefa 100644
--- a/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.hpp
+++ b/apps/peripheral/sensor/optical_flow/paa3905/PAA3905.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2022-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file PAA3905.hpp
@@ -54,85 +31,84 @@ using namespace time_literals;
 using namespace PixArt_PAA3905;
 
 #define DIR_WRITE(a) ((a) | Bit7)
-#define DIR_READ(a) ((a) & 0x7F)
+#define DIR_READ(a)  ((a) & 0x7F)
 
-class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
-{
+class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905> {
 public:
-	PAA3905(const I2CSPIDriverConfig &config);
-	virtual ~PAA3905();
+    PAA3905(const I2CSPIDriverConfig &config);
+    virtual ~PAA3905();
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
+    void exit_and_cleanup() override;
 
-	int probe() override;
+    int probe() override;
 
-	bool Reset();
-	bool Configure();
+    bool Reset();
+    bool Configure();
 
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
 
-	uint8_t RegisterRead(uint8_t reg);
-	void RegisterWrite(uint8_t reg, uint8_t data);
+    uint8_t RegisterRead(uint8_t reg);
+    void    RegisterWrite(uint8_t reg, uint8_t data);
 
-	void ConfigureAutomaticModeSwitching();
-	void ConfigureStandardDetectionSetting();
-	void ConfigureEnhancedDetectionMode();
-	void EnableLed();
+    void ConfigureAutomaticModeSwitching();
+    void ConfigureStandardDetectionSetting();
+    void ConfigureEnhancedDetectionMode();
+    void EnableLed();
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		READ,
-	} _state{STATE::RESET};
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        READ,
+    } _state{STATE::RESET};
 
-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+    uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	const spi_drdy_gpio_t _drdy_gpio;
+    const spi_drdy_gpio_t _drdy_gpio;
 
-	matrix::Dcmf _rotation;
+    matrix::Dcmf _rotation;
 
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	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};
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
+    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};
 
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_publish{0};
-	hrt_abstime _last_motion{0};
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_publish{0};
+    hrt_abstime _last_motion{0};
 
-	int16_t _delta_x_raw_prev{0};
-	int16_t _delta_y_raw_prev{0};
-	uint32_t _shutter_prev{0};
-	uint8_t _quality_prev{0};
-	uint8_t _raw_data_sum_prev{0};
+    int16_t  _delta_x_raw_prev{0};
+    int16_t  _delta_y_raw_prev{0};
+    uint32_t _shutter_prev{0};
+    uint8_t  _quality_prev{0};
+    uint8_t  _raw_data_sum_prev{0};
 
-	int _failure_count{0};
-	int _discard_reading{0};
+    int _failure_count{0};
+    int _discard_reading{0};
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _motion_interrupt_enabled{false};
+    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                     _motion_interrupt_enabled{false};
 
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
-	static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
+    uint32_t                  _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
+    static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
 
-	Mode _mode{Mode::LowLight};
+    Mode _mode{Mode::LowLight};
 
-	hrt_abstime _last_write_time{0};
-	hrt_abstime _last_read_time{0};
+    hrt_abstime _last_write_time{0};
+    hrt_abstime _last_read_time{0};
 };
diff --git a/apps/peripheral/sensor/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp b/apps/peripheral/sensor/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp
index aa642f0b82..c63aa6ca49 100644
--- a/apps/peripheral/sensor/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp
+++ b/apps/peripheral/sensor/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -45,8 +22,7 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace PixArt_PAA3905
-{
+namespace PixArt_PAA3905 {
 
 static constexpr uint8_t PRODUCT_ID         = 0xA2;
 static constexpr uint8_t REVISION_ID        = 0x00;
@@ -56,7 +32,7 @@ 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
+static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000;           // 2MHz SPI serial interface
 
 // Various time delays
 static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
@@ -65,68 +41,68 @@ static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And S
 static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay
 
 enum Register : uint8_t {
-	Product_ID         = 0x00,
-	Revision_ID        = 0x01,
-	Motion             = 0x02,
-	Delta_X_L          = 0x03,
-	Delta_X_H          = 0x04,
-	Delta_Y_L          = 0x05,
-	Delta_Y_H          = 0x06,
-	Squal              = 0x07,
-	RawData_Sum        = 0x08,
-	Maximum_RawData    = 0x09,
-	Minimum_RawData    = 0x0A,
-	Shutter_Lower      = 0x0B,
-	Shutter_Middle     = 0x0C,
-	Shutter_Upper      = 0x0D,
-
-	Observation        = 0x15,
-	Motion_Burst       = 0x16,
-
-	Power_Up_Reset     = 0x3A,
-	Shutdown           = 0x3B,
-
-	Resolution         = 0x4E,
-
-	Inverse_Product_ID = 0x5F,
+    Product_ID      = 0x00,
+    Revision_ID     = 0x01,
+    Motion          = 0x02,
+    Delta_X_L       = 0x03,
+    Delta_X_H       = 0x04,
+    Delta_Y_L       = 0x05,
+    Delta_Y_H       = 0x06,
+    Squal           = 0x07,
+    RawData_Sum     = 0x08,
+    Maximum_RawData = 0x09,
+    Minimum_RawData = 0x0A,
+    Shutter_Lower   = 0x0B,
+    Shutter_Middle  = 0x0C,
+    Shutter_Upper   = 0x0D,
+
+    Observation  = 0x15,
+    Motion_Burst = 0x16,
+
+    Power_Up_Reset = 0x3A,
+    Shutdown       = 0x3B,
+
+    Resolution = 0x4E,
+
+    Inverse_Product_ID = 0x5F,
 };
 
 enum Motion_Bit : uint8_t {
-	MotionOccurred     = Bit7, // Motion since last report
-	ChallengingSurface = Bit0, // Challenging surface is detected
+    MotionOccurred     = Bit7, // Motion since last report
+    ChallengingSurface = Bit0, // Challenging surface is detected
 };
 
 enum Observation_Bit : uint8_t {
-	// Bit [7:6]
-	AMS_mode_0 = 0,
-	AMS_mode_1 = Bit6,
-	AMS_mode_2 = Bit7,
+    // Bit [7:6]
+    AMS_mode_0 = 0,
+    AMS_mode_1 = Bit6,
+    AMS_mode_2 = Bit7,
 
-	// Bit [5:0]
-	WorkingCorrectly = 0x3F,
+    // Bit [5:0]
+    WorkingCorrectly = 0x3F,
 };
 
 enum class Mode {
-	Bright        = 0,
-	LowLight      = 1,
-	SuperLowLight = 2,
+    Bright        = 0,
+    LowLight      = 1,
+    SuperLowLight = 2,
 };
 
 struct BURST_TRANSFER {
-	uint8_t Motion;
-	uint8_t Observation;
-	uint8_t Delta_X_L;
-	uint8_t Delta_X_H;
-	uint8_t Delta_Y_L;
-	uint8_t Delta_Y_H;
-	uint8_t Reserved;
-	uint8_t SQUAL;
-	uint8_t RawData_Sum;
-	uint8_t Maximum_RawData;
-	uint8_t Minimum_RawData;
-	uint8_t Shutter_Upper;
-	uint8_t Shutter_Middle;
-	uint8_t Shutter_Lower;
+    uint8_t Motion;
+    uint8_t Observation;
+    uint8_t Delta_X_L;
+    uint8_t Delta_X_H;
+    uint8_t Delta_Y_L;
+    uint8_t Delta_Y_H;
+    uint8_t Reserved;
+    uint8_t SQUAL;
+    uint8_t RawData_Sum;
+    uint8_t Maximum_RawData;
+    uint8_t Minimum_RawData;
+    uint8_t Shutter_Upper;
+    uint8_t Shutter_Middle;
+    uint8_t Shutter_Lower;
 };
 
-}
+} // namespace PixArt_PAA3905
diff --git a/apps/peripheral/sensor/optical_flow/paa3905/paa3905_main.cpp b/apps/peripheral/sensor/optical_flow/paa3905/paa3905_main.cpp
index 08dfba6b2b..d501104699 100644
--- a/apps/peripheral/sensor/optical_flow/paa3905/paa3905_main.cpp
+++ b/apps/peripheral/sensor/optical_flow/paa3905/paa3905_main.cpp
@@ -1,86 +1,61 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PAA3905.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void PAA3905::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("paa3905", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void PAA3905::print_usage() {
+    PRINT_MODULE_USAGE_NAME("paa3905", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
-{
-	int ch = 0;
-	using ThisDriver = PAA3905;
-	BusCLIArguments cli{false, true};
-	cli.custom1 = -1;
-	cli.spi_mode = SPIDEV_MODE3;
-	cli.default_spi_frequency = SPI_SPEED;
+extern "C" __EXPORT int paa3905_main(int argc, char *argv[]) {
+    int ch           = 0;
+    using ThisDriver = PAA3905;
+    BusCLIArguments cli{false, true};
+    cli.custom1               = -1;
+    cli.spi_mode              = SPIDEV_MODE3;
+    cli.default_spi_frequency = SPI_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
-		switch (ch) {
-		case 'Y':
-			cli.custom1 = atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
+        switch (ch) {
+        case 'Y':
+            cli.custom1 = atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAA3905);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAA3905);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/optical_flow/paa3905/parameters.c b/apps/peripheral/sensor/optical_flow/paa3905/parameters.c
index 2646721d94..5a7461b8df 100644
--- a/apps/peripheral/sensor/optical_flow/paa3905/parameters.c
+++ b/apps/peripheral/sensor/optical_flow/paa3905/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * PAA3905 Optical Flow
diff --git a/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.cpp b/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.cpp
index 05a8092a3d..63e8f29e23 100644
--- a/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.cpp
+++ b/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.cpp
@@ -1,948 +1,902 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PAW3902.hpp"
 
-static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
-{
-	return (msb << 8u) | lsb;
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) {
+    return (msb << 8u) | lsb;
 }
 
 PAW3902::PAW3902(const I2CSPIDriverConfig &config) :
-	SPI(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");
-	}
+    SPI(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;
+    float yaw_rotation_degrees = (float)config.custom1;
 
-	if (yaw_rotation_degrees >= 0.f) {
-		PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
-			 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
+    if (yaw_rotation_degrees >= 0.f) {
+        PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
+                 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
 
-		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
+        _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
 
-	} else {
-		_rotation.identity();
-	}
+    } else {
+        _rotation.identity();
+    }
 }
 
-PAW3902::~PAW3902()
-{
-	perf_free(_bad_register_perf);
-	perf_free(_bad_transfer_perf);
-	perf_free(_reset_perf);
-	perf_free(_false_motion_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);
+PAW3902::~PAW3902() {
+    perf_free(_bad_register_perf);
+    perf_free(_bad_transfer_perf);
+    perf_free(_reset_perf);
+    perf_free(_false_motion_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 PAW3902::init()
-{
-	int ret = SPI::init();
+int PAW3902::init() {
+    int ret = SPI::init();
 
-	if (ret != PX4_OK) {
-		DEVICE_DEBUG("SPI::init failed (%i)", ret);
-		return ret;
-	}
+    if (ret != PX4_OK) {
+        DEVICE_DEBUG("SPI::init failed (%i)", ret);
+        return ret;
+    }
 
-	return Reset() ? 0 : -1;
+    return Reset() ? 0 : -1;
 }
 
-bool PAW3902::Reset()
-{
-	_state = STATE::RESET;
-	DataReadyInterruptDisable();
-	_drdy_timestamp_sample.store(0);
-	ScheduleClear();
-	ScheduleNow();
-	return true;
+bool PAW3902::Reset() {
+    _state = STATE::RESET;
+    DataReadyInterruptDisable();
+    _drdy_timestamp_sample.store(0);
+    ScheduleClear();
+    ScheduleNow();
+    return true;
 }
 
-void PAW3902::exit_and_cleanup()
-{
-	DataReadyInterruptDisable();
-	I2CSPIDriverBase::exit_and_cleanup();
+void PAW3902::exit_and_cleanup() {
+    DataReadyInterruptDisable();
+    I2CSPIDriverBase::exit_and_cleanup();
 }
 
-void PAW3902::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_reset_perf);
-	perf_print_counter(_false_motion_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);
+void PAW3902::print_status() {
+    I2CSPIDriverBase::print_status();
+
+    perf_print_counter(_bad_register_perf);
+    perf_print_counter(_bad_transfer_perf);
+    perf_print_counter(_reset_perf);
+    perf_print_counter(_false_motion_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);
 }
 
-int PAW3902::probe()
-{
-	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);
+int PAW3902::probe() {
+    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) {
-			DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
-			break;
-		}
+        if (Product_ID != PRODUCT_ID) {
+            DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
+            break;
+        }
 
-		if (Revision_ID != REVISION_ID) {
-			DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
-			break;
-		}
+        if (Revision_ID != REVISION_ID) {
+            DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
+            break;
+        }
 
-		if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
-			DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
-			break;
-		}
+        if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+            DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
+            break;
+        }
 
-		return PX4_OK;
-	}
+        return PX4_OK;
+    }
 
-	return PX4_ERROR;
+    return PX4_ERROR;
 }
 
-void PAW3902::RunImpl()
-{
-	const hrt_abstime now = hrt_absolute_time();
-
-	switch (_state) {
-	case STATE::RESET:
-		// Issue a soft reset
-		RegisterWrite(Register::Power_Up_Reset, 0x5A);
-
-		_bright_to_low_counter = 0;
-		_low_to_superlow_counter = 0;
-		_low_to_bright_counter = 0;
-		_superlow_to_low_counter = 0;
-
-		_reset_timestamp = now;
-		_failure_count = 0;
-		_state = STATE::WAIT_FOR_RESET;
-		perf_count(_reset_perf);
-		ScheduleDelayed(1_ms);
-		break;
-
-	case STATE::WAIT_FOR_RESET:
-		if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
-			// 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);
-
-			_discard_reading = 3;
-
-			// if reset succeeded then configure
-			_state = STATE::CONFIGURE;
-			ScheduleNow();
-
-		} else {
-			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Reset failed, retrying");
-				_state = STATE::RESET;
-				ScheduleDelayed(100_ms);
-
-			} else {
-				PX4_DEBUG("Reset not complete, check again in 100 ms");
-				ScheduleDelayed(100_ms);
-			}
-		}
-
-		break;
-
-	case STATE::CONFIGURE:
-		if (Configure()) {
-			// if configure succeeded then start measurement cycle
-			_state = STATE::READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_motion_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(1_s);
-
-			} else {
-				_motion_interrupt_enabled = false;
-				ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
-			}
-
-		} else {
-			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
-				PX4_DEBUG("Configure failed, resetting");
-				_state = STATE::RESET;
-
-			} else {
-				PX4_DEBUG("Configure failed, retrying");
-			}
-
-			ScheduleDelayed(100_ms);
-		}
-
-		break;
-
-	case STATE::READ: {
-			hrt_abstime timestamp_sample = now;
-
-			if (_motion_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(kBackupScheduleIntervalUs);
-			}
-
-			struct TransferBuffer {
-				uint8_t cmd = Register::Motion_Burst;
-				BURST_TRANSFER data{};
-			} buffer{};
-			static_assert(sizeof(buffer) == (12 + 1));
-
-			bool success = false;
-
-			if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
-
-				hrt_store_absolute_time(&_last_read_time);
-
-				if (_discard_reading > 0) {
-					_discard_reading--;
-				}
-
-				if (buffer.data.RawData_Sum > 0x98) {
-					perf_count(_bad_register_perf);
-					PX4_ERR("invalid RawData_Sum > 0x98");
-				}
-
-				// publish sensor_optical_flow
-				sensor_optical_flow_s sensor_optical_flow{};
-				sensor_optical_flow.timestamp_sample = timestamp_sample;
-				sensor_optical_flow.device_id = get_device_id();
-
-				sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
-
-				// set specs according to datasheet
-				sensor_optical_flow.max_flow_rate = 7.4f;           // Datasheet: 7.4 rad/s
-				sensor_optical_flow.min_ground_distance = 0.08f;    // Datasheet: 80mm
-				sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
-
-				// 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 ≥ 0x1FF0
-				// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x1FF0
-				// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
-
-				// 13-bit Shutter register
-				const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
-				const uint8_t shutter_lower = buffer.data.Shutter_Lower;
-
-				const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
-
-				// Number of Features = SQUAL * 4
-				// RawData_Sum maximum register value is 0x98
-				bool data_valid = (buffer.data.SQUAL > 0)
-						  && (buffer.data.RawData_Sum > 0)
-						  && (buffer.data.RawData_Sum <= 0x98)
-						  && (shutter > 0)
-						  && (_discard_reading == 0);
-
-				switch (_mode) {
-				case Mode::Bright:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_BRIGHT;
-
-					// quality < 25 (0x19) and shutter >= 8176 (0x1FF0)
-					if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					// shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C)
-					if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x3C)) {
-						// Bright -> LowLight
-						_bright_to_low_counter++;
-
-						if (_bright_to_low_counter >= 10) {
-							_mode = Mode::LowLight;
-							Reset();
-						}
-
-					} else {
-						_bright_to_low_counter = 0;
-					}
-
-					break;
-
-				case Mode::LowLight:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
-
-					// quality < 70 (0x46) and shutter >= 8176 (0x1FF0)
-					if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					// shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A)
-					if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x5A)) {
-						// LowLight -> SuperLowLight
-						_low_to_bright_counter = 0;
-						_low_to_superlow_counter++;
-
-						if (_low_to_superlow_counter >= 10) {
-							_mode = Mode::SuperLowLight;
-							Reset();
-						}
-
-					} else if (shutter < 0x0BB8) {
-						// LowLight -> Bright
-						//  shutter < 0x0BB8 (3000)
-						_low_to_bright_counter++;
-						_low_to_superlow_counter = 0;
-
-						if (_low_to_bright_counter >= 10) {
-							_mode = Mode::Bright;
-							Reset();
-						}
-
-					} else {
-						_low_to_bright_counter = 0;
-						_low_to_superlow_counter = 0;
-					}
-
-					break;
-
-				case Mode::SuperLowLight:
-					sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
-					sensor_optical_flow.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
-
-					// quality < 85 (0x55) and shutter >= 3008 (0x0BC0)
-					if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
-						// false motion report, discarding
-						data_valid = false;
-						perf_count(_false_motion_perf);
-					}
-
-					// shutter < 500 (0x01F4)
-					if (shutter < 0x01F4) {
-						// should not operate with Shutter < 0x01F4 in Mode 2
-						_superlow_to_low_counter++;
-						data_valid = false;
-
-					} else if (shutter < 0x03E8) {
-						// SuperLowLight -> LowLight
-						//  shutter < 1000 (0x03E8)
-						_superlow_to_low_counter++;
-					}
-
-					if (_superlow_to_low_counter >= 10) {
-						_mode = Mode::LowLight;
-						Reset();
-					}
-
-					break;
-				}
-
-				// motion in burst transfer
-				const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
-
-				const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
-				const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
-
-				if (data_valid) {
-
-					const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
-					const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
-
-					bool publish = false;
-
-					if (motion_reported) {
-						// 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);
-
-						sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
-						sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
-
-						sensor_optical_flow.quality = buffer.data.SQUAL;
-
-						publish = true;
-
-						_last_motion = timestamp_sample;
-
-					} else if (zero_flow && (timestamp_sample > _last_motion)) {
-						// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
-						const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
-										|| (shutter != _shutter_prev)
-										|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
-										|| (buffer.data.SQUAL != _quality_prev);
-
-						if (burst_read_changed) {
-
-							sensor_optical_flow.pixel_flow[0] = 0;
-							sensor_optical_flow.pixel_flow[1] = 0;
-
-							sensor_optical_flow.quality = buffer.data.SQUAL;
-
-							publish = true;
-						}
-					}
-
-					// only publish when there's valid data or on timeout
-					if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
-
-						sensor_optical_flow.timestamp = hrt_absolute_time();
-						_sensor_optical_flow_pub.publish(sensor_optical_flow);
+void PAW3902::RunImpl() {
+    const hrt_abstime now = hrt_absolute_time();
+
+    switch (_state) {
+    case STATE::RESET:
+        // Issue a soft reset
+        RegisterWrite(Register::Power_Up_Reset, 0x5A);
 
-						_last_publish = sensor_optical_flow.timestamp_sample;
-					}
+        _bright_to_low_counter   = 0;
+        _low_to_superlow_counter = 0;
+        _low_to_bright_counter   = 0;
+        _superlow_to_low_counter = 0;
 
-					// backup schedule if we're reliant on the motion interrupt and there's very little flow
-					if (_motion_interrupt_enabled && little_to_no_flow) {
-						switch (_mode) {
-						case Mode::Bright:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
-							break;
+        _reset_timestamp = now;
+        _failure_count   = 0;
+        _state           = STATE::WAIT_FOR_RESET;
+        perf_count(_reset_perf);
+        ScheduleDelayed(1_ms);
+        break;
 
-						case Mode::LowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
-							break;
+    case STATE::WAIT_FOR_RESET:
+        if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
+            // 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);
 
-						case Mode::SuperLowLight:
-							ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
-							break;
-						}
-					}
+            _discard_reading = 3;
+
+            // if reset succeeded then configure
+            _state = STATE::CONFIGURE;
+            ScheduleNow();
+
+        } else {
+            // RESET not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Reset failed, retrying");
+                _state = STATE::RESET;
+                ScheduleDelayed(100_ms);
 
-					success = true;
+            } else {
+                PX4_DEBUG("Reset not complete, check again in 100 ms");
+                ScheduleDelayed(100_ms);
+            }
+        }
+
+        break;
+
+    case STATE::CONFIGURE:
+        if (Configure()) {
+            // if configure succeeded then start measurement cycle
+            _state = STATE::READ;
+
+            if (DataReadyInterruptConfigure()) {
+                _motion_interrupt_enabled = true;
+
+                // backup schedule as a watchdog timeout
+                ScheduleDelayed(1_s);
+
+            } else {
+                _motion_interrupt_enabled = false;
+                ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
+            }
+
+        } else {
+            // CONFIGURE not complete
+            if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+                PX4_DEBUG("Configure failed, resetting");
+                _state = STATE::RESET;
+
+            } else {
+                PX4_DEBUG("Configure failed, retrying");
+            }
+
+            ScheduleDelayed(100_ms);
+        }
+
+        break;
+
+    case STATE::READ: {
+        hrt_abstime timestamp_sample = now;
+
+        if (_motion_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(kBackupScheduleIntervalUs);
+        }
+
+        struct TransferBuffer {
+            uint8_t        cmd = Register::Motion_Burst;
+            BURST_TRANSFER data{};
+        } buffer{};
+
+        static_assert(sizeof(buffer) == (12 + 1));
+
+        bool success = false;
+
+        if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
+            hrt_store_absolute_time(&_last_read_time);
+
+            if (_discard_reading > 0) {
+                _discard_reading--;
+            }
+
+            if (buffer.data.RawData_Sum > 0x98) {
+                perf_count(_bad_register_perf);
+                PX4_ERR("invalid RawData_Sum > 0x98");
+            }
+
+            // publish sensor_optical_flow
+            sensor_optical_flow_s sensor_optical_flow{};
+            sensor_optical_flow.timestamp_sample = timestamp_sample;
+            sensor_optical_flow.device_id        = get_device_id();
+
+            sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
+
+            // set specs according to datasheet
+            sensor_optical_flow.max_flow_rate       = 7.4f;     // Datasheet: 7.4 rad/s
+            sensor_optical_flow.min_ground_distance = 0.08f;    // Datasheet: 80mm
+            sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
+
+            // 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 ≥ 0x1FF0
+            // Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x1FF0
+            // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
+
+            // 13-bit Shutter register
+            const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
+            const uint8_t shutter_lower = buffer.data.Shutter_Lower;
+
+            const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
+
+            // Number of Features = SQUAL * 4
+            // RawData_Sum maximum register value is 0x98
+            bool data_valid = (buffer.data.SQUAL > 0)
+                           && (buffer.data.RawData_Sum > 0)
+                           && (buffer.data.RawData_Sum <= 0x98)
+                           && (shutter > 0)
+                           && (_discard_reading == 0);
+
+            switch (_mode) {
+            case Mode::Bright:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_BRIGHT;
+
+                // quality < 25 (0x19) and shutter >= 8176 (0x1FF0)
+                if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                // shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C)
+                if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x3C)) {
+                    // Bright -> LowLight
+                    _bright_to_low_counter++;
+
+                    if (_bright_to_low_counter >= 10) {
+                        _mode = Mode::LowLight;
+                        Reset();
+                    }
+
+                } else {
+                    _bright_to_low_counter = 0;
+                }
+
+                break;
+
+            case Mode::LowLight:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_LOWLIGHT;
+
+                // quality < 70 (0x46) and shutter >= 8176 (0x1FF0)
+                if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                // shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A)
+                if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x5A)) {
+                    // LowLight -> SuperLowLight
+                    _low_to_bright_counter = 0;
+                    _low_to_superlow_counter++;
+
+                    if (_low_to_superlow_counter >= 10) {
+                        _mode = Mode::SuperLowLight;
+                        Reset();
+                    }
+
+                } else if (shutter < 0x0BB8) {
+                    // LowLight -> Bright
+                    //  shutter < 0x0BB8 (3000)
+                    _low_to_bright_counter++;
+                    _low_to_superlow_counter = 0;
+
+                    if (_low_to_bright_counter >= 10) {
+                        _mode = Mode::Bright;
+                        Reset();
+                    }
+
+                } else {
+                    _low_to_bright_counter   = 0;
+                    _low_to_superlow_counter = 0;
+                }
+
+                break;
+
+            case Mode::SuperLowLight:
+                sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
+                sensor_optical_flow.mode                    = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
+
+                // quality < 85 (0x55) and shutter >= 3008 (0x0BC0)
+                if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
+                    // false motion report, discarding
+                    data_valid = false;
+                    perf_count(_false_motion_perf);
+                }
+
+                // shutter < 500 (0x01F4)
+                if (shutter < 0x01F4) {
+                    // should not operate with Shutter < 0x01F4 in Mode 2
+                    _superlow_to_low_counter++;
+                    data_valid = false;
+
+                } else if (shutter < 0x03E8) {
+                    // SuperLowLight -> LowLight
+                    //  shutter < 1000 (0x03E8)
+                    _superlow_to_low_counter++;
+                }
+
+                if (_superlow_to_low_counter >= 10) {
+                    _mode = Mode::LowLight;
+                    Reset();
+                }
+
+                break;
+            }
+
+            // motion in burst transfer
+            const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
+
+            const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
+            const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
+
+            if (data_valid) {
+                const bool zero_flow         = (delta_x_raw == 0) && (delta_y_raw == 0);
+                const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
+
+                bool publish = false;
+
+                if (motion_reported) {
+                    // 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);
+
+                    sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
+                    sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+
+                    sensor_optical_flow.quality = buffer.data.SQUAL;
+
+                    publish = true;
+
+                    _last_motion = timestamp_sample;
+
+                } else if (zero_flow && (timestamp_sample > _last_motion)) {
+                    // no motion, but burst read looks valid and we should have seen new data by now if there was any motion
+                    const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
+                                                 || (shutter != _shutter_prev)
+                                                 || (buffer.data.RawData_Sum != _raw_data_sum_prev)
+                                                 || (buffer.data.SQUAL != _quality_prev);
+
+                    if (burst_read_changed) {
+                        sensor_optical_flow.pixel_flow[0] = 0;
+                        sensor_optical_flow.pixel_flow[1] = 0;
+
+                        sensor_optical_flow.quality = buffer.data.SQUAL;
+
+                        publish = true;
+                    }
+                }
+
+                // only publish when there's valid data or on timeout
+                if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
+                    sensor_optical_flow.timestamp = hrt_absolute_time();
+                    _sensor_optical_flow_pub.publish(sensor_optical_flow);
+
+                    _last_publish = sensor_optical_flow.timestamp_sample;
+                }
 
-					if (_failure_count > 0) {
-						_failure_count--;
-					}
-				}
+                // backup schedule if we're reliant on the motion interrupt and there's very little flow
+                if (_motion_interrupt_enabled && little_to_no_flow) {
+                    switch (_mode) {
+                    case Mode::Bright:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
+                        break;
+
+                    case Mode::LowLight:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
+                        break;
+
+                    case Mode::SuperLowLight:
+                        ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
+                        break;
+                    }
+                }
+
+                success = true;
+
+                if (_failure_count > 0) {
+                    _failure_count--;
+                }
+            }
 
-				_delta_x_raw_prev = delta_x_raw;
-				_delta_y_raw_prev = delta_y_raw;
-				_shutter_prev = shutter;
-				_raw_data_sum_prev = buffer.data.RawData_Sum;
-				_quality_prev = buffer.data.SQUAL;
+            _delta_x_raw_prev  = delta_x_raw;
+            _delta_y_raw_prev  = delta_y_raw;
+            _shutter_prev      = shutter;
+            _raw_data_sum_prev = buffer.data.RawData_Sum;
+            _quality_prev      = buffer.data.SQUAL;
 
-			} else {
-				perf_count(_bad_transfer_perf);
-			}
+        } else {
+            perf_count(_bad_transfer_perf);
+        }
 
-			if (!success) {
-				_failure_count++;
+        if (!success) {
+            _failure_count++;
 
-				// full reset if things are failing consistently
-				if (_failure_count > 10) {
-					Reset();
-				}
-			}
-		}
+            // full reset if things are failing consistently
+            if (_failure_count > 10) {
+                Reset();
+            }
+        }
+    }
 
-		break;
-	}
+    break;
+    }
 }
 
-bool PAW3902::Configure()
-{
-	switch (_mode) {
-	case Mode::Bright:
-		ConfigureModeBright();
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
-		perf_count(_mode_change_bright_perf);
-		break;
-
-	case Mode::LowLight:
-		ConfigureModeLowLight();
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
-		perf_count(_mode_change_low_light_perf);
-		break;
-
-	case Mode::SuperLowLight:
-		ConfigureModeSuperLowLight();
-		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
-		perf_count(_mode_change_super_low_light_perf);
-		break;
-	}
-
-	EnableLed();
-
-	return true;
+bool PAW3902::Configure() {
+    switch (_mode) {
+    case Mode::Bright:
+        ConfigureModeBright();
+        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
+        perf_count(_mode_change_bright_perf);
+        break;
+
+    case Mode::LowLight:
+        ConfigureModeLowLight();
+        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
+        perf_count(_mode_change_low_light_perf);
+        break;
+
+    case Mode::SuperLowLight:
+        ConfigureModeSuperLowLight();
+        _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
+        perf_count(_mode_change_super_low_light_perf);
+        break;
+    }
+
+    EnableLed();
+
+    return true;
 }
 
-void PAW3902::ConfigureModeBright()
-{
-	// Mode 0: Bright (126 fps) 60 Lux typical
-
-	// set performance optimization registers
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x55, 0x01);
-	RegisterWrite(0x50, 0x07);
-	RegisterWrite(0x7f, 0x0e);
-	RegisterWrite(0x43, 0x10);
-
-	RegisterWrite(0x48, 0x02);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x51, 0x7b);
-	RegisterWrite(0x50, 0x00);
-	RegisterWrite(0x55, 0x00);
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x61, 0xAD);
-	RegisterWrite(0x7F, 0x03);
-	RegisterWrite(0x40, 0x00);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x41, 0xB3);
-	RegisterWrite(0x43, 0xF1);
-	RegisterWrite(0x45, 0x14);
-	RegisterWrite(0x5F, 0x34);
-	RegisterWrite(0x7B, 0x08);
-	RegisterWrite(0x5e, 0x34);
-	RegisterWrite(0x5b, 0x32);
-	RegisterWrite(0x6d, 0x32);
-	RegisterWrite(0x45, 0x17);
-	RegisterWrite(0x70, 0xe5);
-	RegisterWrite(0x71, 0xe5);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x44, 0x1B);
-	RegisterWrite(0x40, 0xBF);
-	RegisterWrite(0x4E, 0x3F);
-	RegisterWrite(0x7F, 0x08);
-	RegisterWrite(0x66, 0x44);
-	RegisterWrite(0x65, 0x20);
-	RegisterWrite(0x6a, 0x3a);
-	RegisterWrite(0x61, 0x05);
-	RegisterWrite(0x62, 0x05);
-	RegisterWrite(0x7F, 0x09);
-	RegisterWrite(0x4F, 0xAF);
-	RegisterWrite(0x48, 0x80);
-	RegisterWrite(0x49, 0x80);
-	RegisterWrite(0x57, 0x77);
-	RegisterWrite(0x5F, 0x40);
-	RegisterWrite(0x60, 0x78);
-	RegisterWrite(0x61, 0x78);
-	RegisterWrite(0x62, 0x08);
-	RegisterWrite(0x63, 0x50);
-	RegisterWrite(0x7F, 0x0A);
-	RegisterWrite(0x45, 0x60);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x4D, 0x11);
-	RegisterWrite(0x55, 0x80);
-	RegisterWrite(0x74, 0x21);
-	RegisterWrite(0x75, 0x1F);
-	RegisterWrite(0x4A, 0x78);
-	RegisterWrite(0x4B, 0x78);
-	RegisterWrite(0x44, 0x08);
-	RegisterWrite(0x45, 0x50);
-	RegisterWrite(0x64, 0xFE);
-	RegisterWrite(0x65, 0x1F);
-	RegisterWrite(0x72, 0x0A);
-	RegisterWrite(0x73, 0x00);
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x44, 0x84);
-	RegisterWrite(0x65, 0x47);
-	RegisterWrite(0x66, 0x18);
-	RegisterWrite(0x63, 0x70);
-	RegisterWrite(0x6f, 0x2c);
-	RegisterWrite(0x7F, 0x15);
-	RegisterWrite(0x48, 0x48);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x41, 0x0D);
-	RegisterWrite(0x43, 0x14);
-	RegisterWrite(0x4B, 0x0E);
-	RegisterWrite(0x45, 0x0F);
-	RegisterWrite(0x44, 0x42);
-	RegisterWrite(0x4C, 0x80);
-	RegisterWrite(0x7F, 0x10);
-	RegisterWrite(0x5B, 0x03);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x41);
-
-	px4_usleep(10'000); // delay 10ms
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x32, 0x00);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x40);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x68, 0x70);
-	RegisterWrite(0x69, 0x01);
-	RegisterWrite(0x7F, 0x0D);
-	RegisterWrite(0x48, 0xC0);
-	RegisterWrite(0x6F, 0xD5);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x5B, 0xA0);
-	RegisterWrite(0x4E, 0xA8);
-	RegisterWrite(0x5A, 0x50);
-	RegisterWrite(0x40, 0x80);
-	RegisterWrite(0x73, 0x1f);
-
-	px4_usleep(10'000); // delay 10ms
-
-	RegisterWrite(0x73, 0x00);
+void PAW3902::ConfigureModeBright() {
+    // Mode 0: Bright (126 fps) 60 Lux typical
+
+    // set performance optimization registers
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x55, 0x01);
+    RegisterWrite(0x50, 0x07);
+    RegisterWrite(0x7f, 0x0e);
+    RegisterWrite(0x43, 0x10);
+
+    RegisterWrite(0x48, 0x02);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x51, 0x7b);
+    RegisterWrite(0x50, 0x00);
+    RegisterWrite(0x55, 0x00);
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x61, 0xAD);
+    RegisterWrite(0x7F, 0x03);
+    RegisterWrite(0x40, 0x00);
+    RegisterWrite(0x7F, 0x05);
+    RegisterWrite(0x41, 0xB3);
+    RegisterWrite(0x43, 0xF1);
+    RegisterWrite(0x45, 0x14);
+    RegisterWrite(0x5F, 0x34);
+    RegisterWrite(0x7B, 0x08);
+    RegisterWrite(0x5e, 0x34);
+    RegisterWrite(0x5b, 0x32);
+    RegisterWrite(0x6d, 0x32);
+    RegisterWrite(0x45, 0x17);
+    RegisterWrite(0x70, 0xe5);
+    RegisterWrite(0x71, 0xe5);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x44, 0x1B);
+    RegisterWrite(0x40, 0xBF);
+    RegisterWrite(0x4E, 0x3F);
+    RegisterWrite(0x7F, 0x08);
+    RegisterWrite(0x66, 0x44);
+    RegisterWrite(0x65, 0x20);
+    RegisterWrite(0x6a, 0x3a);
+    RegisterWrite(0x61, 0x05);
+    RegisterWrite(0x62, 0x05);
+    RegisterWrite(0x7F, 0x09);
+    RegisterWrite(0x4F, 0xAF);
+    RegisterWrite(0x48, 0x80);
+    RegisterWrite(0x49, 0x80);
+    RegisterWrite(0x57, 0x77);
+    RegisterWrite(0x5F, 0x40);
+    RegisterWrite(0x60, 0x78);
+    RegisterWrite(0x61, 0x78);
+    RegisterWrite(0x62, 0x08);
+    RegisterWrite(0x63, 0x50);
+    RegisterWrite(0x7F, 0x0A);
+    RegisterWrite(0x45, 0x60);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x4D, 0x11);
+    RegisterWrite(0x55, 0x80);
+    RegisterWrite(0x74, 0x21);
+    RegisterWrite(0x75, 0x1F);
+    RegisterWrite(0x4A, 0x78);
+    RegisterWrite(0x4B, 0x78);
+    RegisterWrite(0x44, 0x08);
+    RegisterWrite(0x45, 0x50);
+    RegisterWrite(0x64, 0xFE);
+    RegisterWrite(0x65, 0x1F);
+    RegisterWrite(0x72, 0x0A);
+    RegisterWrite(0x73, 0x00);
+    RegisterWrite(0x7F, 0x14);
+    RegisterWrite(0x44, 0x84);
+    RegisterWrite(0x65, 0x47);
+    RegisterWrite(0x66, 0x18);
+    RegisterWrite(0x63, 0x70);
+    RegisterWrite(0x6f, 0x2c);
+    RegisterWrite(0x7F, 0x15);
+    RegisterWrite(0x48, 0x48);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x41, 0x0D);
+    RegisterWrite(0x43, 0x14);
+    RegisterWrite(0x4B, 0x0E);
+    RegisterWrite(0x45, 0x0F);
+    RegisterWrite(0x44, 0x42);
+    RegisterWrite(0x4C, 0x80);
+    RegisterWrite(0x7F, 0x10);
+    RegisterWrite(0x5B, 0x03);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x41);
+
+    px4_usleep(10'000); // delay 10ms
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x32, 0x00);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x40);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x68, 0x70);
+    RegisterWrite(0x69, 0x01);
+    RegisterWrite(0x7F, 0x0D);
+    RegisterWrite(0x48, 0xC0);
+    RegisterWrite(0x6F, 0xD5);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x5B, 0xA0);
+    RegisterWrite(0x4E, 0xA8);
+    RegisterWrite(0x5A, 0x50);
+    RegisterWrite(0x40, 0x80);
+    RegisterWrite(0x73, 0x1f);
+
+    px4_usleep(10'000); // delay 10ms
+
+    RegisterWrite(0x73, 0x00);
 }
 
-void PAW3902::ConfigureModeLowLight()
-{
-	// Mode 1: Low Light (126 fps) 30 Lux typical
-	// low light and low speed motion tracking
-
-	// set performance optimization registers
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x55, 0x01);
-	RegisterWrite(0x50, 0x07);
-	RegisterWrite(0x7f, 0x0e);
-	RegisterWrite(0x43, 0x10);
-
-	RegisterWrite(0x48, 0x02);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x51, 0x7b);
-	RegisterWrite(0x50, 0x00);
-	RegisterWrite(0x55, 0x00);
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x61, 0xAD);
-	RegisterWrite(0x7F, 0x03);
-	RegisterWrite(0x40, 0x00);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x41, 0xB3);
-	RegisterWrite(0x43, 0xF1);
-	RegisterWrite(0x45, 0x14);
-	RegisterWrite(0x5F, 0x34);
-	RegisterWrite(0x7B, 0x08);
-	RegisterWrite(0x5e, 0x34);
-	RegisterWrite(0x5b, 0x65);
-	RegisterWrite(0x6d, 0x65);
-	RegisterWrite(0x45, 0x17);
-	RegisterWrite(0x70, 0xe5);
-	RegisterWrite(0x71, 0xe5);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x44, 0x1B);
-	RegisterWrite(0x40, 0xBF);
-	RegisterWrite(0x4E, 0x3F);
-	RegisterWrite(0x7F, 0x08);
-	RegisterWrite(0x66, 0x44);
-	RegisterWrite(0x65, 0x20);
-	RegisterWrite(0x6a, 0x3a);
-	RegisterWrite(0x61, 0x05);
-	RegisterWrite(0x62, 0x05);
-	RegisterWrite(0x7F, 0x09);
-	RegisterWrite(0x4F, 0xAF);
-	RegisterWrite(0x48, 0x80);
-	RegisterWrite(0x49, 0x80);
-	RegisterWrite(0x57, 0x77);
-	RegisterWrite(0x5F, 0x40);
-	RegisterWrite(0x60, 0x78);
-	RegisterWrite(0x61, 0x78);
-	RegisterWrite(0x62, 0x08);
-	RegisterWrite(0x63, 0x50);
-	RegisterWrite(0x7F, 0x0A);
-	RegisterWrite(0x45, 0x60);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x4D, 0x11);
-	RegisterWrite(0x55, 0x80);
-	RegisterWrite(0x74, 0x21);
-	RegisterWrite(0x75, 0x1F);
-	RegisterWrite(0x4A, 0x78);
-	RegisterWrite(0x4B, 0x78);
-	RegisterWrite(0x44, 0x08);
-	RegisterWrite(0x45, 0x50);
-	RegisterWrite(0x64, 0xFE);
-	RegisterWrite(0x65, 0x1F);
-	RegisterWrite(0x72, 0x0A);
-	RegisterWrite(0x73, 0x00);
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x44, 0x84);
-	RegisterWrite(0x65, 0x67);
-	RegisterWrite(0x66, 0x18);
-	RegisterWrite(0x63, 0x70);
-	RegisterWrite(0x6f, 0x2c);
-	RegisterWrite(0x7F, 0x15);
-	RegisterWrite(0x48, 0x48);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x41, 0x0D);
-	RegisterWrite(0x43, 0x14);
-	RegisterWrite(0x4B, 0x0E);
-	RegisterWrite(0x45, 0x0F);
-	RegisterWrite(0x44, 0x42);
-	RegisterWrite(0x4C, 0x80);
-	RegisterWrite(0x7F, 0x10);
-	RegisterWrite(0x5B, 0x03);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x41);
-
-	px4_usleep(10'000); // delay 10ms
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x32, 0x00);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x40);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x68, 0x70);
-	RegisterWrite(0x69, 0x01);
-	RegisterWrite(0x7F, 0x0D);
-	RegisterWrite(0x48, 0xC0);
-	RegisterWrite(0x6F, 0xD5);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x5B, 0xA0);
-	RegisterWrite(0x4E, 0xA8);
-	RegisterWrite(0x5A, 0x50);
-	RegisterWrite(0x40, 0x80);
-	RegisterWrite(0x73, 0x1f);
-
-	px4_usleep(10'000); // delay 10ms
-
-	RegisterWrite(0x73, 0x00);
+void PAW3902::ConfigureModeLowLight() {
+    // Mode 1: Low Light (126 fps) 30 Lux typical
+    // low light and low speed motion tracking
+
+    // set performance optimization registers
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x55, 0x01);
+    RegisterWrite(0x50, 0x07);
+    RegisterWrite(0x7f, 0x0e);
+    RegisterWrite(0x43, 0x10);
+
+    RegisterWrite(0x48, 0x02);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x51, 0x7b);
+    RegisterWrite(0x50, 0x00);
+    RegisterWrite(0x55, 0x00);
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x61, 0xAD);
+    RegisterWrite(0x7F, 0x03);
+    RegisterWrite(0x40, 0x00);
+    RegisterWrite(0x7F, 0x05);
+    RegisterWrite(0x41, 0xB3);
+    RegisterWrite(0x43, 0xF1);
+    RegisterWrite(0x45, 0x14);
+    RegisterWrite(0x5F, 0x34);
+    RegisterWrite(0x7B, 0x08);
+    RegisterWrite(0x5e, 0x34);
+    RegisterWrite(0x5b, 0x65);
+    RegisterWrite(0x6d, 0x65);
+    RegisterWrite(0x45, 0x17);
+    RegisterWrite(0x70, 0xe5);
+    RegisterWrite(0x71, 0xe5);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x44, 0x1B);
+    RegisterWrite(0x40, 0xBF);
+    RegisterWrite(0x4E, 0x3F);
+    RegisterWrite(0x7F, 0x08);
+    RegisterWrite(0x66, 0x44);
+    RegisterWrite(0x65, 0x20);
+    RegisterWrite(0x6a, 0x3a);
+    RegisterWrite(0x61, 0x05);
+    RegisterWrite(0x62, 0x05);
+    RegisterWrite(0x7F, 0x09);
+    RegisterWrite(0x4F, 0xAF);
+    RegisterWrite(0x48, 0x80);
+    RegisterWrite(0x49, 0x80);
+    RegisterWrite(0x57, 0x77);
+    RegisterWrite(0x5F, 0x40);
+    RegisterWrite(0x60, 0x78);
+    RegisterWrite(0x61, 0x78);
+    RegisterWrite(0x62, 0x08);
+    RegisterWrite(0x63, 0x50);
+    RegisterWrite(0x7F, 0x0A);
+    RegisterWrite(0x45, 0x60);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x4D, 0x11);
+    RegisterWrite(0x55, 0x80);
+    RegisterWrite(0x74, 0x21);
+    RegisterWrite(0x75, 0x1F);
+    RegisterWrite(0x4A, 0x78);
+    RegisterWrite(0x4B, 0x78);
+    RegisterWrite(0x44, 0x08);
+    RegisterWrite(0x45, 0x50);
+    RegisterWrite(0x64, 0xFE);
+    RegisterWrite(0x65, 0x1F);
+    RegisterWrite(0x72, 0x0A);
+    RegisterWrite(0x73, 0x00);
+    RegisterWrite(0x7F, 0x14);
+    RegisterWrite(0x44, 0x84);
+    RegisterWrite(0x65, 0x67);
+    RegisterWrite(0x66, 0x18);
+    RegisterWrite(0x63, 0x70);
+    RegisterWrite(0x6f, 0x2c);
+    RegisterWrite(0x7F, 0x15);
+    RegisterWrite(0x48, 0x48);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x41, 0x0D);
+    RegisterWrite(0x43, 0x14);
+    RegisterWrite(0x4B, 0x0E);
+    RegisterWrite(0x45, 0x0F);
+    RegisterWrite(0x44, 0x42);
+    RegisterWrite(0x4C, 0x80);
+    RegisterWrite(0x7F, 0x10);
+    RegisterWrite(0x5B, 0x03);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x41);
+
+    px4_usleep(10'000); // delay 10ms
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x32, 0x00);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x40);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x68, 0x70);
+    RegisterWrite(0x69, 0x01);
+    RegisterWrite(0x7F, 0x0D);
+    RegisterWrite(0x48, 0xC0);
+    RegisterWrite(0x6F, 0xD5);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x5B, 0xA0);
+    RegisterWrite(0x4E, 0xA8);
+    RegisterWrite(0x5A, 0x50);
+    RegisterWrite(0x40, 0x80);
+    RegisterWrite(0x73, 0x1f);
+
+    px4_usleep(10'000); // delay 10ms
+
+    RegisterWrite(0x73, 0x00);
 }
 
-void PAW3902::ConfigureModeSuperLowLight()
-{
-	// Mode 2: Super Low Light (50 fps) 9 Lux typical
-	// super low light and low speed motion tracking
-
-	// set performance optimization registers
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x55, 0x01);
-	RegisterWrite(0x50, 0x07);
-	RegisterWrite(0x7f, 0x0e);
-	RegisterWrite(0x43, 0x10);
-
-	RegisterWrite(0x48, 0x04);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x51, 0x7b);
-	RegisterWrite(0x50, 0x00);
-	RegisterWrite(0x55, 0x00);
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x61, 0xAD);
-	RegisterWrite(0x7F, 0x03);
-	RegisterWrite(0x40, 0x00);
-	RegisterWrite(0x7F, 0x05);
-	RegisterWrite(0x41, 0xB3);
-	RegisterWrite(0x43, 0xF1);
-	RegisterWrite(0x45, 0x14);
-	RegisterWrite(0x5F, 0x34);
-	RegisterWrite(0x7B, 0x08);
-	RegisterWrite(0x5E, 0x34);
-	RegisterWrite(0x5B, 0x32);
-	RegisterWrite(0x6D, 0x32);
-	RegisterWrite(0x45, 0x17);
-	RegisterWrite(0x70, 0xE5);
-	RegisterWrite(0x71, 0xE5);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x44, 0x1B);
-	RegisterWrite(0x40, 0xBF);
-	RegisterWrite(0x4E, 0x3F);
-	RegisterWrite(0x7F, 0x08);
-	RegisterWrite(0x66, 0x44);
-	RegisterWrite(0x65, 0x20);
-	RegisterWrite(0x6A, 0x3a);
-	RegisterWrite(0x61, 0x05);
-	RegisterWrite(0x62, 0x05);
-	RegisterWrite(0x7F, 0x09);
-	RegisterWrite(0x4F, 0xAF);
-	RegisterWrite(0x48, 0x80);
-	RegisterWrite(0x49, 0x80);
-	RegisterWrite(0x57, 0x77);
-	RegisterWrite(0x5F, 0x40);
-	RegisterWrite(0x60, 0x78);
-	RegisterWrite(0x61, 0x78);
-	RegisterWrite(0x62, 0x08);
-	RegisterWrite(0x63, 0x50);
-	RegisterWrite(0x7F, 0x0A);
-	RegisterWrite(0x45, 0x60);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x4D, 0x11);
-	RegisterWrite(0x55, 0x80);
-	RegisterWrite(0x74, 0x21);
-	RegisterWrite(0x75, 0x1F);
-	RegisterWrite(0x4A, 0x78);
-	RegisterWrite(0x4B, 0x78);
-	RegisterWrite(0x44, 0x08);
-	RegisterWrite(0x45, 0x50);
-	RegisterWrite(0x64, 0xCE);
-	RegisterWrite(0x65, 0x0B);
-	RegisterWrite(0x72, 0x0A);
-	RegisterWrite(0x73, 0x00);
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x44, 0x84);
-	RegisterWrite(0x65, 0x67);
-	RegisterWrite(0x66, 0x18);
-	RegisterWrite(0x63, 0x70);
-	RegisterWrite(0x6f, 0x2c);
-	RegisterWrite(0x7F, 0x15);
-	RegisterWrite(0x48, 0x48);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x41, 0x0D);
-	RegisterWrite(0x43, 0x14);
-	RegisterWrite(0x4B, 0x0E);
-	RegisterWrite(0x45, 0x0F);
-	RegisterWrite(0x44, 0x42);
-	RegisterWrite(0x4C, 0x80);
-	RegisterWrite(0x7F, 0x10);
-	RegisterWrite(0x5B, 0x02);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x41);
-
-	px4_usleep(25'000); // delay 25ms
-
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x32, 0x44);
-	RegisterWrite(0x7F, 0x07);
-	RegisterWrite(0x40, 0x40);
-	RegisterWrite(0x7F, 0x06);
-	RegisterWrite(0x68, 0x40);
-	RegisterWrite(0x69, 0x02);
-	RegisterWrite(0x7F, 0x0D);
-	RegisterWrite(0x48, 0xC0);
-	RegisterWrite(0x6F, 0xD5);
-	RegisterWrite(0x7F, 0x00);
-	RegisterWrite(0x5B, 0xA0);
-	RegisterWrite(0x4E, 0xA8);
-	RegisterWrite(0x5A, 0x50);
-	RegisterWrite(0x40, 0x80);
-	RegisterWrite(0x73, 0x0B);
-
-	px4_usleep(25'000); // delay 25ms
-
-	RegisterWrite(0x73, 0x00);
+void PAW3902::ConfigureModeSuperLowLight() {
+    // Mode 2: Super Low Light (50 fps) 9 Lux typical
+    // super low light and low speed motion tracking
+
+    // set performance optimization registers
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x55, 0x01);
+    RegisterWrite(0x50, 0x07);
+    RegisterWrite(0x7f, 0x0e);
+    RegisterWrite(0x43, 0x10);
+
+    RegisterWrite(0x48, 0x04);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x51, 0x7b);
+    RegisterWrite(0x50, 0x00);
+    RegisterWrite(0x55, 0x00);
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x61, 0xAD);
+    RegisterWrite(0x7F, 0x03);
+    RegisterWrite(0x40, 0x00);
+    RegisterWrite(0x7F, 0x05);
+    RegisterWrite(0x41, 0xB3);
+    RegisterWrite(0x43, 0xF1);
+    RegisterWrite(0x45, 0x14);
+    RegisterWrite(0x5F, 0x34);
+    RegisterWrite(0x7B, 0x08);
+    RegisterWrite(0x5E, 0x34);
+    RegisterWrite(0x5B, 0x32);
+    RegisterWrite(0x6D, 0x32);
+    RegisterWrite(0x45, 0x17);
+    RegisterWrite(0x70, 0xE5);
+    RegisterWrite(0x71, 0xE5);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x44, 0x1B);
+    RegisterWrite(0x40, 0xBF);
+    RegisterWrite(0x4E, 0x3F);
+    RegisterWrite(0x7F, 0x08);
+    RegisterWrite(0x66, 0x44);
+    RegisterWrite(0x65, 0x20);
+    RegisterWrite(0x6A, 0x3a);
+    RegisterWrite(0x61, 0x05);
+    RegisterWrite(0x62, 0x05);
+    RegisterWrite(0x7F, 0x09);
+    RegisterWrite(0x4F, 0xAF);
+    RegisterWrite(0x48, 0x80);
+    RegisterWrite(0x49, 0x80);
+    RegisterWrite(0x57, 0x77);
+    RegisterWrite(0x5F, 0x40);
+    RegisterWrite(0x60, 0x78);
+    RegisterWrite(0x61, 0x78);
+    RegisterWrite(0x62, 0x08);
+    RegisterWrite(0x63, 0x50);
+    RegisterWrite(0x7F, 0x0A);
+    RegisterWrite(0x45, 0x60);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x4D, 0x11);
+    RegisterWrite(0x55, 0x80);
+    RegisterWrite(0x74, 0x21);
+    RegisterWrite(0x75, 0x1F);
+    RegisterWrite(0x4A, 0x78);
+    RegisterWrite(0x4B, 0x78);
+    RegisterWrite(0x44, 0x08);
+    RegisterWrite(0x45, 0x50);
+    RegisterWrite(0x64, 0xCE);
+    RegisterWrite(0x65, 0x0B);
+    RegisterWrite(0x72, 0x0A);
+    RegisterWrite(0x73, 0x00);
+    RegisterWrite(0x7F, 0x14);
+    RegisterWrite(0x44, 0x84);
+    RegisterWrite(0x65, 0x67);
+    RegisterWrite(0x66, 0x18);
+    RegisterWrite(0x63, 0x70);
+    RegisterWrite(0x6f, 0x2c);
+    RegisterWrite(0x7F, 0x15);
+    RegisterWrite(0x48, 0x48);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x41, 0x0D);
+    RegisterWrite(0x43, 0x14);
+    RegisterWrite(0x4B, 0x0E);
+    RegisterWrite(0x45, 0x0F);
+    RegisterWrite(0x44, 0x42);
+    RegisterWrite(0x4C, 0x80);
+    RegisterWrite(0x7F, 0x10);
+    RegisterWrite(0x5B, 0x02);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x41);
+
+    px4_usleep(25'000); // delay 25ms
+
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x32, 0x44);
+    RegisterWrite(0x7F, 0x07);
+    RegisterWrite(0x40, 0x40);
+    RegisterWrite(0x7F, 0x06);
+    RegisterWrite(0x68, 0x40);
+    RegisterWrite(0x69, 0x02);
+    RegisterWrite(0x7F, 0x0D);
+    RegisterWrite(0x48, 0xC0);
+    RegisterWrite(0x6F, 0xD5);
+    RegisterWrite(0x7F, 0x00);
+    RegisterWrite(0x5B, 0xA0);
+    RegisterWrite(0x4E, 0xA8);
+    RegisterWrite(0x5A, 0x50);
+    RegisterWrite(0x40, 0x80);
+    RegisterWrite(0x73, 0x0B);
+
+    px4_usleep(25'000); // delay 25ms
+
+    RegisterWrite(0x73, 0x00);
 }
 
-void PAW3902::EnableLed()
-{
-	// Enable LED_N controls
-	RegisterWrite(0x7F, 0x14);
-	RegisterWrite(0x6F, 0x1c);
-	RegisterWrite(0x7F, 0x00);
+void PAW3902::EnableLed() {
+    // Enable LED_N controls
+    RegisterWrite(0x7F, 0x14);
+    RegisterWrite(0x6F, 0x1c);
+    RegisterWrite(0x7F, 0x00);
 }
 
-int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg)
-{
-	static_cast<PAW3902 *>(arg)->DataReady();
-	return 0;
+int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) {
+    static_cast<PAW3902 *>(arg)->DataReady();
+    return 0;
 }
 
-void PAW3902::DataReady()
-{
-	_drdy_timestamp_sample.store(hrt_absolute_time());
-	ScheduleNow();
+void PAW3902::DataReady() {
+    _drdy_timestamp_sample.store(hrt_absolute_time());
+    ScheduleNow();
 }
 
-bool PAW3902::DataReadyInterruptConfigure()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool PAW3902::DataReadyInterruptConfigure() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	// Setup data ready on falling edge
-	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
+    // Setup data ready on falling edge
+    return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
 }
 
-bool PAW3902::DataReadyInterruptDisable()
-{
-	if (_drdy_gpio == 0) {
-		return false;
-	}
+bool PAW3902::DataReadyInterruptDisable() {
+    if (_drdy_gpio == 0) {
+        return false;
+    }
 
-	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
+    return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
 }
 
-uint8_t PAW3902::RegisterRead(uint8_t reg)
-{
-	// tSWR SPI Time Between Write And Read Commands
-	const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
+uint8_t PAW3902::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);
-	}
+    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);
+    // 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);
-	}
+    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);
+    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];
+    return cmd[1];
 }
 
-void PAW3902::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);
+void PAW3902::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);
-	}
+    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;
-	transfer(&cmd[0], nullptr, sizeof(cmd));
-	hrt_store_absolute_time(&_last_write_time);
+    uint8_t cmd[2];
+    cmd[0] = DIR_WRITE(reg);
+    cmd[1] = data;
+    transfer(&cmd[0], nullptr, sizeof(cmd));
+    hrt_store_absolute_time(&_last_write_time);
 }
diff --git a/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.hpp b/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.hpp
index b2e568d619..ac6cb23202 100644
--- a/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.hpp
+++ b/apps/peripheral/sensor/optical_flow/paw3902/PAW3902.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019-2023 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file PAW3902.hpp
@@ -54,90 +31,89 @@ using namespace time_literals;
 using namespace PixArt_PAW3902;
 
 #define DIR_WRITE(a) ((a) | Bit7)
-#define DIR_READ(a) ((a) & 0x7F)
+#define DIR_READ(a)  ((a) & 0x7F)
 
-class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
-{
+class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902> {
 public:
-	PAW3902(const I2CSPIDriverConfig &config);
-	virtual ~PAW3902();
+    PAW3902(const I2CSPIDriverConfig &config);
+    virtual ~PAW3902();
 
-	static void print_usage();
+    static void print_usage();
 
-	void RunImpl();
+    void RunImpl();
 
-	int init() override;
-	void print_status() override;
+    int  init() override;
+    void print_status() override;
 
 private:
-	void exit_and_cleanup() override;
+    void exit_and_cleanup() override;
 
-	int probe() override;
+    int probe() override;
 
-	bool Reset();
-	bool Configure();
+    bool Reset();
+    bool Configure();
 
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-	bool DataReadyInterruptConfigure();
-	bool DataReadyInterruptDisable();
+    static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+    void       DataReady();
+    bool       DataReadyInterruptConfigure();
+    bool       DataReadyInterruptDisable();
 
-	uint8_t RegisterRead(uint8_t reg);
-	void RegisterWrite(uint8_t reg, uint8_t data);
+    uint8_t RegisterRead(uint8_t reg);
+    void    RegisterWrite(uint8_t reg, uint8_t data);
 
-	void ConfigureModeBright();
-	void ConfigureModeLowLight();
-	void ConfigureModeSuperLowLight();
-	void EnableLed();
+    void ConfigureModeBright();
+    void ConfigureModeLowLight();
+    void ConfigureModeSuperLowLight();
+    void EnableLed();
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		READ,
-	} _state{STATE::RESET};
+    enum class STATE : uint8_t {
+        RESET,
+        WAIT_FOR_RESET,
+        CONFIGURE,
+        READ,
+    } _state{STATE::RESET};
 
-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+    uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	const spi_drdy_gpio_t _drdy_gpio;
+    const spi_drdy_gpio_t _drdy_gpio;
 
-	matrix::Dcmf _rotation;
+    matrix::Dcmf _rotation;
 
-	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	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};
+    perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad register")};
+    perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad transfer")};
+    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};
 
-	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _last_publish{0};
-	hrt_abstime _last_motion{0};
+    hrt_abstime _reset_timestamp{0};
+    hrt_abstime _last_publish{0};
+    hrt_abstime _last_motion{0};
 
-	int16_t _delta_x_raw_prev{0};
-	int16_t _delta_y_raw_prev{0};
-	uint16_t _shutter_prev{0};
-	uint8_t _quality_prev{0};
-	uint8_t _raw_data_sum_prev{0};
+    int16_t  _delta_x_raw_prev{0};
+    int16_t  _delta_y_raw_prev{0};
+    uint16_t _shutter_prev{0};
+    uint8_t  _quality_prev{0};
+    uint8_t  _raw_data_sum_prev{0};
 
-	int _failure_count{0};
-	int _discard_reading{0};
+    int _failure_count{0};
+    int _discard_reading{0};
 
-	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
-	bool _motion_interrupt_enabled{false};
+    px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+    bool                     _motion_interrupt_enabled{false};
 
-	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
-	static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
+    uint32_t                  _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
+    static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
 
-	Mode _mode{Mode::LowLight};
+    Mode _mode{Mode::LowLight};
 
-	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 _bright_to_low_counter{0};
+    int _low_to_superlow_counter{0};
+    int _low_to_bright_counter{0};
+    int _superlow_to_low_counter{0};
 
-	hrt_abstime _last_write_time{0};
-	hrt_abstime _last_read_time{0};
+    hrt_abstime _last_write_time{0};
+    hrt_abstime _last_read_time{0};
 };
diff --git a/apps/peripheral/sensor/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp b/apps/peripheral/sensor/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp
index 5d78a4025f..a816f4634a 100644
--- a/apps/peripheral/sensor/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp
+++ b/apps/peripheral/sensor/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -45,8 +22,7 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-namespace PixArt_PAW3902
-{
+namespace PixArt_PAW3902 {
 
 static constexpr uint8_t PRODUCT_ID         = 0x49;
 static constexpr uint8_t REVISION_ID        = 0x01;
@@ -56,7 +32,7 @@ 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
+static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000;           // 2MHz SPI serial interface
 
 // Various time delays
 static constexpr uint32_t TIME_TSWW_us      = 11; // SPI Time Between Write Commands (actually 10.5us)
@@ -65,53 +41,53 @@ static constexpr uint32_t TIME_TSRW_TSRR_us = 2;  // SPI Time Between Read And S
 static constexpr uint32_t TIME_TSRAD_us     = 2;  // SPI Read Address-Data Delay
 
 enum Register : uint8_t {
-	Product_ID         = 0x00,
-	Revision_ID        = 0x01,
-	Motion             = 0x02,
-	Delta_X_L          = 0x03,
-	Delta_X_H          = 0x04,
-	Delta_Y_L          = 0x05,
-	Delta_Y_H          = 0x06,
-	Squal              = 0x07,
-	RawData_Sum        = 0x08,
-	Maximum_RawData    = 0x09,
-	Minimum_RawData    = 0x0A,
-	Shutter_Lower      = 0x0B,
-	Shutter_Upper      = 0x0C,
-
-	Observation        = 0x15,
-	Motion_Burst       = 0x16,
-
-	Power_Up_Reset     = 0x3A,
-
-	Resolution         = 0x4E,
-
-	Inverse_Product_ID = 0x5F,
+    Product_ID      = 0x00,
+    Revision_ID     = 0x01,
+    Motion          = 0x02,
+    Delta_X_L       = 0x03,
+    Delta_X_H       = 0x04,
+    Delta_Y_L       = 0x05,
+    Delta_Y_H       = 0x06,
+    Squal           = 0x07,
+    RawData_Sum     = 0x08,
+    Maximum_RawData = 0x09,
+    Minimum_RawData = 0x0A,
+    Shutter_Lower   = 0x0B,
+    Shutter_Upper   = 0x0C,
+
+    Observation  = 0x15,
+    Motion_Burst = 0x16,
+
+    Power_Up_Reset = 0x3A,
+
+    Resolution = 0x4E,
+
+    Inverse_Product_ID = 0x5F,
 };
 
 enum Motion_Bit : uint8_t {
-	MOT = Bit7, // Motion since last report
+    MOT = Bit7, // Motion since last report
 };
 
 enum class Mode {
-	Bright        = 0,
-	LowLight      = 1,
-	SuperLowLight = 2,
+    Bright        = 0,
+    LowLight      = 1,
+    SuperLowLight = 2,
 };
 
 struct BURST_TRANSFER {
-	uint8_t Motion;
-	uint8_t Observation;
-	uint8_t Delta_X_L;
-	uint8_t Delta_X_H;
-	uint8_t Delta_Y_L;
-	uint8_t Delta_Y_H;
-	uint8_t SQUAL;
-	uint8_t RawData_Sum;
-	uint8_t Maximum_RawData;
-	uint8_t Minimum_RawData;
-	uint8_t Shutter_Upper;
-	uint8_t Shutter_Lower;
+    uint8_t Motion;
+    uint8_t Observation;
+    uint8_t Delta_X_L;
+    uint8_t Delta_X_H;
+    uint8_t Delta_Y_L;
+    uint8_t Delta_Y_H;
+    uint8_t SQUAL;
+    uint8_t RawData_Sum;
+    uint8_t Maximum_RawData;
+    uint8_t Minimum_RawData;
+    uint8_t Shutter_Upper;
+    uint8_t Shutter_Lower;
 };
 
-}
+} // namespace PixArt_PAW3902
diff --git a/apps/peripheral/sensor/optical_flow/paw3902/parameters.c b/apps/peripheral/sensor/optical_flow/paw3902/parameters.c
index e2ce1da533..7566728bc8 100644
--- a/apps/peripheral/sensor/optical_flow/paw3902/parameters.c
+++ b/apps/peripheral/sensor/optical_flow/paw3902/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * PAW3902/PAW3903 Optical Flow
diff --git a/apps/peripheral/sensor/optical_flow/paw3902/paw3902_main.cpp b/apps/peripheral/sensor/optical_flow/paw3902/paw3902_main.cpp
index e3eb857dc2..2479e1d120 100644
--- a/apps/peripheral/sensor/optical_flow/paw3902/paw3902_main.cpp
+++ b/apps/peripheral/sensor/optical_flow/paw3902/paw3902_main.cpp
@@ -1,86 +1,61 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PAW3902.hpp"
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 
-void PAW3902::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("paw3902", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void PAW3902::print_usage() {
+    PRINT_MODULE_USAGE_NAME("paw3902", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
-{
-	int ch = 0;
-	using ThisDriver = PAW3902;
-	BusCLIArguments cli{false, true};
-	cli.custom1 = -1;
-	cli.spi_mode = SPIDEV_MODE3;
-	cli.default_spi_frequency = SPI_SPEED;
+extern "C" __EXPORT int paw3902_main(int argc, char *argv[]) {
+    int ch           = 0;
+    using ThisDriver = PAW3902;
+    BusCLIArguments cli{false, true};
+    cli.custom1               = -1;
+    cli.spi_mode              = SPIDEV_MODE3;
+    cli.default_spi_frequency = SPI_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
-		switch (ch) {
-		case 'Y':
-			cli.custom1 = atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
+        switch (ch) {
+        case 'Y':
+            cli.custom1 = atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAW3902);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAW3902);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.cpp b/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.cpp
index 7c5506c81d..420e6e3671 100644
--- a/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.cpp
+++ b/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.cpp
@@ -1,406 +1,360 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PMW3901.hpp"
 
 static constexpr uint32_t TIME_us_TSWW = 11; //  - actually 10.5us
 
 PMW3901::PMW3901(const I2CSPIDriverConfig &config) :
-	SPI(config),
-	I2CSPIDriver(config),
-	_sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")),
-	_comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")),
-	_yaw_rotation(config.rotation)
-{
+    SPI(config),
+    I2CSPIDriver(config),
+    _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")),
+    _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")),
+    _yaw_rotation(config.rotation) {
 }
 
-PMW3901::~PMW3901()
-{
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
+PMW3901::~PMW3901() {
+    // free perf counters
+    perf_free(_sample_perf);
+    perf_free(_comms_errors);
 }
 
-int
-PMW3901::sensorInit()
-{
-	uint8_t data[5] {};
-
-	// Power on reset
-	writeRegister(0x3A, 0x5A);
-	usleep(5000);
-
-	// Reading the motion registers one time
-	readRegister(0x02, &data[0], 1);
-	readRegister(0x03, &data[1], 1);
-	readRegister(0x04, &data[2], 1);
-	readRegister(0x05, &data[3], 1);
-	readRegister(0x06, &data[4], 1);
-
-	usleep(1000);
-
-	// set performance optimization registers
-	// from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018)
-	unsigned char v = 0;
-	unsigned char c1 = 0;
-	unsigned char c2 = 0;
-
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x55, 0x01);
-	writeRegister(0x50, 0x07);
-	writeRegister(0x7f, 0x0e);
-	writeRegister(0x43, 0x10);
-
-	readRegister(0x67, &v, 1);
-
-	// if bit7 is set
-	if (v & (1 << 7)) {
-		writeRegister(0x48, 0x04);
-
-	} else {
-		writeRegister(0x48, 0x02);
-	}
-
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x51, 0x7b);
-	writeRegister(0x50, 0x00);
-	writeRegister(0x55, 0x00);
-
-	writeRegister(0x7F, 0x0e);
-	readRegister(0x73, &v, 1);
-
-	if (v == 0) {
-		readRegister(0x70, &c1, 1);
-
-		if (c1 <= 28) {
-			c1 = c1 + 14;
-
-		} else {
-			c1 = c1 + 11;
-		}
-
-		if (c1 > 0x3F) {
-			c1 = 0x3F;
-		}
-
-		readRegister(0x71, &c2, 1);
-		c2 = ((unsigned short)c2 * 45) / 100;
-
-		writeRegister(0x7f, 0x00);
-		writeRegister(0x61, 0xAD);
-		writeRegister(0x51, 0x70);
-		writeRegister(0x7f, 0x0e);
-		writeRegister(0x70, c1);
-		writeRegister(0x71, c2);
-	}
-
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x61, 0xAD);
-	writeRegister(0x7F, 0x03);
-	writeRegister(0x40, 0x00);
-	writeRegister(0x7F, 0x05);
-	writeRegister(0x41, 0xB3);
-	writeRegister(0x43, 0xF1);
-	writeRegister(0x45, 0x14);
-	writeRegister(0x5B, 0x32);
-	writeRegister(0x5F, 0x34);
-	writeRegister(0x7B, 0x08);
-	writeRegister(0x7F, 0x06);
-	writeRegister(0x44, 0x1B);
-	writeRegister(0x40, 0xBF);
-	writeRegister(0x4E, 0x3F);
-	writeRegister(0x7F, 0x08);
-	writeRegister(0x65, 0x20);
-	writeRegister(0x6A, 0x18);
-	writeRegister(0x7F, 0x09);
-	writeRegister(0x4F, 0xAF);
-	writeRegister(0x5F, 0x40);
-	writeRegister(0x48, 0x80);
-	writeRegister(0x49, 0x80);
-	writeRegister(0x57, 0x77);
-	writeRegister(0x60, 0x78);
-	writeRegister(0x61, 0x78);
-	writeRegister(0x62, 0x08);
-	writeRegister(0x63, 0x50);
-	writeRegister(0x7F, 0x0A);
-	writeRegister(0x45, 0x60);
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x4D, 0x11);
-	writeRegister(0x55, 0x80);
-	writeRegister(0x74, 0x21);
-	writeRegister(0x75, 0x1F);
-	writeRegister(0x4A, 0x78);
-	writeRegister(0x4B, 0x78);
-	writeRegister(0x44, 0x08);
-	writeRegister(0x45, 0x50);
-	writeRegister(0x64, 0xFF);
-	writeRegister(0x65, 0x1F);
-	writeRegister(0x7F, 0x14);
-	writeRegister(0x65, 0x67);
-	writeRegister(0x66, 0x08);
-	writeRegister(0x63, 0x70);
-	writeRegister(0x7F, 0x15);
-	writeRegister(0x48, 0x48);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x41, 0x0D);
-	writeRegister(0x43, 0x14);
-	writeRegister(0x4B, 0x0E);
-	writeRegister(0x45, 0x0F);
-	writeRegister(0x44, 0x42);
-	writeRegister(0x4C, 0x80);
-	writeRegister(0x7F, 0x10);
-	writeRegister(0x5B, 0x02);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x40, 0x41);
-	writeRegister(0x70, 0x00);
-
-	px4_usleep(10000); // delay 10ms
-
-	writeRegister(0x32, 0x44);
-	writeRegister(0x7F, 0x07);
-	writeRegister(0x40, 0x40);
-	writeRegister(0x7F, 0x06);
-	writeRegister(0x62, 0xF0);
-	writeRegister(0x63, 0x00);
-	writeRegister(0x7F, 0x0D);
-	writeRegister(0x48, 0xC0);
-	writeRegister(0x6F, 0xD5);
-	writeRegister(0x7F, 0x00);
-	writeRegister(0x5B, 0xA0);
-	writeRegister(0x4E, 0xA8);
-	writeRegister(0x5A, 0x50);
-	writeRegister(0x40, 0x80);
-
-	return PX4_OK;
+int PMW3901::sensorInit() {
+    uint8_t data[5]{};
+
+    // Power on reset
+    writeRegister(0x3A, 0x5A);
+    usleep(5000);
+
+    // Reading the motion registers one time
+    readRegister(0x02, &data[0], 1);
+    readRegister(0x03, &data[1], 1);
+    readRegister(0x04, &data[2], 1);
+    readRegister(0x05, &data[3], 1);
+    readRegister(0x06, &data[4], 1);
+
+    usleep(1000);
+
+    // set performance optimization registers
+    // from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018)
+    unsigned char v  = 0;
+    unsigned char c1 = 0;
+    unsigned char c2 = 0;
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x55, 0x01);
+    writeRegister(0x50, 0x07);
+    writeRegister(0x7f, 0x0e);
+    writeRegister(0x43, 0x10);
+
+    readRegister(0x67, &v, 1);
+
+    // if bit7 is set
+    if (v & (1 << 7)) {
+        writeRegister(0x48, 0x04);
+
+    } else {
+        writeRegister(0x48, 0x02);
+    }
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x51, 0x7b);
+    writeRegister(0x50, 0x00);
+    writeRegister(0x55, 0x00);
+
+    writeRegister(0x7F, 0x0e);
+    readRegister(0x73, &v, 1);
+
+    if (v == 0) {
+        readRegister(0x70, &c1, 1);
+
+        if (c1 <= 28) {
+            c1 = c1 + 14;
+
+        } else {
+            c1 = c1 + 11;
+        }
+
+        if (c1 > 0x3F) {
+            c1 = 0x3F;
+        }
+
+        readRegister(0x71, &c2, 1);
+        c2 = ((unsigned short)c2 * 45) / 100;
+
+        writeRegister(0x7f, 0x00);
+        writeRegister(0x61, 0xAD);
+        writeRegister(0x51, 0x70);
+        writeRegister(0x7f, 0x0e);
+        writeRegister(0x70, c1);
+        writeRegister(0x71, c2);
+    }
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x61, 0xAD);
+    writeRegister(0x7F, 0x03);
+    writeRegister(0x40, 0x00);
+    writeRegister(0x7F, 0x05);
+    writeRegister(0x41, 0xB3);
+    writeRegister(0x43, 0xF1);
+    writeRegister(0x45, 0x14);
+    writeRegister(0x5B, 0x32);
+    writeRegister(0x5F, 0x34);
+    writeRegister(0x7B, 0x08);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x44, 0x1B);
+    writeRegister(0x40, 0xBF);
+    writeRegister(0x4E, 0x3F);
+    writeRegister(0x7F, 0x08);
+    writeRegister(0x65, 0x20);
+    writeRegister(0x6A, 0x18);
+    writeRegister(0x7F, 0x09);
+    writeRegister(0x4F, 0xAF);
+    writeRegister(0x5F, 0x40);
+    writeRegister(0x48, 0x80);
+    writeRegister(0x49, 0x80);
+    writeRegister(0x57, 0x77);
+    writeRegister(0x60, 0x78);
+    writeRegister(0x61, 0x78);
+    writeRegister(0x62, 0x08);
+    writeRegister(0x63, 0x50);
+    writeRegister(0x7F, 0x0A);
+    writeRegister(0x45, 0x60);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x4D, 0x11);
+    writeRegister(0x55, 0x80);
+    writeRegister(0x74, 0x21);
+    writeRegister(0x75, 0x1F);
+    writeRegister(0x4A, 0x78);
+    writeRegister(0x4B, 0x78);
+    writeRegister(0x44, 0x08);
+    writeRegister(0x45, 0x50);
+    writeRegister(0x64, 0xFF);
+    writeRegister(0x65, 0x1F);
+    writeRegister(0x7F, 0x14);
+    writeRegister(0x65, 0x67);
+    writeRegister(0x66, 0x08);
+    writeRegister(0x63, 0x70);
+    writeRegister(0x7F, 0x15);
+    writeRegister(0x48, 0x48);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x41, 0x0D);
+    writeRegister(0x43, 0x14);
+    writeRegister(0x4B, 0x0E);
+    writeRegister(0x45, 0x0F);
+    writeRegister(0x44, 0x42);
+    writeRegister(0x4C, 0x80);
+    writeRegister(0x7F, 0x10);
+    writeRegister(0x5B, 0x02);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x41);
+    writeRegister(0x70, 0x00);
+
+    px4_usleep(10000); // delay 10ms
+
+    writeRegister(0x32, 0x44);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x40);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x62, 0xF0);
+    writeRegister(0x63, 0x00);
+    writeRegister(0x7F, 0x0D);
+    writeRegister(0x48, 0xC0);
+    writeRegister(0x6F, 0xD5);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x5B, 0xA0);
+    writeRegister(0x4E, 0xA8);
+    writeRegister(0x5A, 0x50);
+    writeRegister(0x40, 0x80);
+
+    return PX4_OK;
 }
 
-int
-PMW3901::init()
-{
-	/* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
-	SPI::set_lockmode(LOCK_THREADS);
+int PMW3901::init() {
+    /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
+    SPI::set_lockmode(LOCK_THREADS);
 
-	/* do SPI init (and probe) first */
-	if (SPI::init() != OK) {
-		return PX4_ERROR;
-	}
+    /* do SPI init (and probe) first */
+    if (SPI::init() != OK) {
+        return PX4_ERROR;
+    }
 
-	sensorInit();
+    sensorInit();
 
-	_previous_collect_timestamp = hrt_absolute_time();
+    _previous_collect_timestamp = hrt_absolute_time();
 
-	start();
+    start();
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-int
-PMW3901::probe()
-{
-	uint8_t data[2] {};
+int PMW3901::probe() {
+    uint8_t data[2]{};
 
-	readRegister(0x00, &data[0], 1); // chip id
+    readRegister(0x00, &data[0], 1); // chip id
 
-	// Test the SPI communication, checking chipId and inverse chipId
-	if (data[0] == 0x49) {
-		return OK;
-	}
+    // Test the SPI communication, checking chipId and inverse chipId
+    if (data[0] == 0x49) {
+        return OK;
+    }
 
-	// not found on any address
-	return -EIO;
+    // not found on any address
+    return -EIO;
 }
 
-int
-PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count)
-{
-	uint8_t cmd[5];	// read up to 4 bytes
+int PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) {
+    uint8_t cmd[5]; // read up to 4 bytes
 
-	cmd[0] = DIR_READ(reg);
+    cmd[0] = DIR_READ(reg);
 
-	int ret = transfer(&cmd[0], &cmd[0], count + 1);
+    int ret = transfer(&cmd[0], &cmd[0], count + 1);
 
-	if (OK != ret) {
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
-	}
+    if (OK != ret) {
+        perf_count(_comms_errors);
+        DEVICE_LOG("spi::transfer returned %d", ret);
+        return ret;
+    }
 
-	memcpy(&data[0], &cmd[1], count);
+    memcpy(&data[0], &cmd[1], count);
 
-	return ret;
+    return ret;
 }
 
-int
-PMW3901::writeRegister(unsigned reg, uint8_t data)
-{
-	uint8_t cmd[2]; 						// write 1 byte
-	int ret;
+int PMW3901::writeRegister(unsigned reg, uint8_t data) {
+    uint8_t cmd[2]; // write 1 byte
+    int     ret;
 
-	cmd[0] = DIR_WRITE(reg);
-	cmd[1] = data;
+    cmd[0] = DIR_WRITE(reg);
+    cmd[1] = data;
 
-	ret = transfer(&cmd[0], nullptr, 2);
+    ret = transfer(&cmd[0], nullptr, 2);
 
-	if (OK != ret) {
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
-	}
+    if (OK != ret) {
+        perf_count(_comms_errors);
+        DEVICE_LOG("spi::transfer returned %d", ret);
+        return ret;
+    }
 
-	px4_usleep(TIME_us_TSWW);
+    px4_usleep(TIME_us_TSWW);
 
-	return ret;
+    return ret;
 }
 
-void
-PMW3901::RunImpl()
-{
-	perf_begin(_sample_perf);
+void PMW3901::RunImpl() {
+    perf_begin(_sample_perf);
 
-	int16_t delta_x_raw = 0;
-	int16_t delta_y_raw = 0;
-	uint8_t qual = 0;
-	float delta_x = 0.0f;
-	float delta_y = 0.0f;
+    int16_t delta_x_raw = 0;
+    int16_t delta_y_raw = 0;
+    uint8_t qual        = 0;
+    float   delta_x     = 0.0f;
+    float   delta_y     = 0.0f;
 
-	uint64_t timestamp = hrt_absolute_time();
-	uint64_t dt_flow = timestamp - _previous_collect_timestamp;
-	_previous_collect_timestamp = timestamp;
+    uint64_t timestamp          = hrt_absolute_time();
+    uint64_t dt_flow            = timestamp - _previous_collect_timestamp;
+    _previous_collect_timestamp = timestamp;
 
-	_flow_dt_sum_usec += dt_flow;
+    _flow_dt_sum_usec += dt_flow;
 
-	readMotionCount(delta_x_raw, delta_y_raw, qual);
+    readMotionCount(delta_x_raw, delta_y_raw, qual);
 
-	if (qual > 0) {
-		_flow_sum_x += delta_x_raw;
-		_flow_sum_y += delta_y_raw;
-		_flow_sample_counter ++;
-		_flow_quality_sum += qual;
-	}
+    if (qual > 0) {
+        _flow_sum_x += delta_x_raw;
+        _flow_sum_y += delta_y_raw;
+        _flow_sample_counter++;
+        _flow_quality_sum += qual;
+    }
 
-	// returns if the collect time has not been reached
-	if (_flow_dt_sum_usec < _collect_time) {
-		return;
-	}
+    // returns if the collect time has not been reached
+    if (_flow_dt_sum_usec < _collect_time) {
+        return;
+    }
 
-	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
+    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
 
-	sensor_optical_flow_s report{};
-	report.timestamp_sample = timestamp;
+    sensor_optical_flow_s report{};
+    report.timestamp_sample = timestamp;
 
-	report.pixel_flow[0] = static_cast<float>(delta_x);
-	report.pixel_flow[1] = 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
-	float zeroval = 0.0f;
-	rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
+    // rotate measurements in yaw from sensor frame to body frame
+    float zeroval = 0.0f;
+    rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
 
-	report.integration_timespan_us = _flow_dt_sum_usec; 	// microseconds
+    report.integration_timespan_us = _flow_dt_sum_usec; // microseconds
 
-	report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
+    report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0;
 
-	/* No gyro on this board */
-	report.delta_angle[0] = NAN;
-	report.delta_angle[1] = NAN;
-	report.delta_angle[2] = NAN;
+    /* No gyro on this board */
+    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
-	report.min_ground_distance = 0.1f; // Datasheet: 80mm
-	report.max_ground_distance = 30.0f; // Datasheet: infinity
+    // set (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
 
-	_flow_dt_sum_usec = 0;
-	_flow_sum_x = 0;
-	_flow_sum_y = 0;
-	_flow_sample_counter = 0;
-	_flow_quality_sum = 0;
+    _flow_dt_sum_usec    = 0;
+    _flow_sum_x          = 0;
+    _flow_sum_y          = 0;
+    _flow_sample_counter = 0;
+    _flow_quality_sum    = 0;
 
-	report.timestamp = hrt_absolute_time();
-	_sensor_optical_flow_pub.publish(report);
+    report.timestamp = hrt_absolute_time();
+    _sensor_optical_flow_pub.publish(report);
 
-	perf_end(_sample_perf);
+    perf_end(_sample_perf);
 }
 
-int
-PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
-{
-	uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
-			     DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0
-			   };
+int PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual) {
+    uint8_t data[12] = {DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
+                        DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0};
 
-	int ret = transfer(&data[0], &data[0], 12);
+    int ret = transfer(&data[0], &data[0], 12);
 
-	if (OK != ret) {
-		qual = 0;
-		perf_count(_comms_errors);
-		DEVICE_LOG("spi::transfer returned %d", ret);
-		return ret;
-	}
+    if (OK != ret) {
+        qual = 0;
+        perf_count(_comms_errors);
+        DEVICE_LOG("spi::transfer returned %d", ret);
+        return ret;
+    }
 
-	deltaX = ((int16_t)data[5] << 8) | data[3];
-	deltaY = ((int16_t)data[9] << 8) | data[7];
+    deltaX = ((int16_t)data[5] << 8) | data[3];
+    deltaY = ((int16_t)data[9] << 8) | data[7];
 
-	// If the reported flow is impossibly large, we just got garbage from the SPI
-	if (deltaX > 240 || deltaY > 240 || deltaX < -240 || deltaY < -240) {
-		qual = 0;
+    // If the reported flow is impossibly large, we just got garbage from the SPI
+    if (deltaX > 240 || deltaY > 240 || deltaX < -240 || deltaY < -240) {
+        qual = 0;
 
-	} else {
-		qual = data[11];
-	}
+    } else {
+        qual = data[11];
+    }
 
-	ret = OK;
+    ret = OK;
 
-	return ret;
+    return ret;
 }
 
-void
-PMW3901::start()
-{
-	// schedule a cycle to start things
-	ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US);
+void PMW3901::start() {
+    // schedule a cycle to start things
+    ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US);
 }
 
-void
-PMW3901::stop()
-{
-	ScheduleClear();
+void PMW3901::stop() {
+    ScheduleClear();
 }
 
-void
-PMW3901::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+void PMW3901::print_status() {
+    I2CSPIDriverBase::print_status();
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
 }
diff --git a/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.hpp b/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.hpp
index 2c845fbdcb..d500898636 100644
--- a/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.hpp
+++ b/apps/peripheral/sensor/optical_flow/pmw3901/PMW3901.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file PMW3901.hpp
@@ -54,67 +31,66 @@
 
 #define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
 
-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
+#define DIR_WRITE(a)          ((a) | (1 << 7))
+#define DIR_READ(a)           ((a) & 0x7f)
 
 /* PMW3901 Registers addresses */
-#define PMW3901_US 1000 /*   1 ms */
+#define PMW3901_US              1000  /*   1 ms */
 #define PMW3901_SAMPLE_INTERVAL 10000 /*  10 ms */
 
-class PMW3901 : public device::SPI, public I2CSPIDriver<PMW3901>
-{
+class PMW3901 : public device::SPI, public I2CSPIDriver<PMW3901> {
 public:
-	PMW3901(const I2CSPIDriverConfig &config);
+    PMW3901(const I2CSPIDriverConfig &config);
 
-	virtual ~PMW3901();
+    virtual ~PMW3901();
 
-	static void print_usage();
+    static void print_usage();
 
-	virtual int init();
+    virtual int init();
 
-	void print_status();
+    void print_status();
 
-	void RunImpl();
+    void RunImpl();
 
 protected:
-	virtual int probe();
+    virtual int probe();
 
 private:
+    const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)
 
-	const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz)
-
-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_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;
+    perf_counter_t _sample_perf;
+    perf_counter_t _comms_errors;
 
-	uint64_t _previous_collect_timestamp{0};
+    uint64_t _previous_collect_timestamp{0};
 
-	enum Rotation _yaw_rotation {};
+    enum Rotation _yaw_rotation {
+    };
 
-	int _flow_sum_x{0};
-	int _flow_sum_y{0};
-	uint64_t _flow_dt_sum_usec{0};
-	uint16_t _flow_quality_sum{0};
-	uint8_t _flow_sample_counter{0};
+    int      _flow_sum_x{0};
+    int      _flow_sum_y{0};
+    uint64_t _flow_dt_sum_usec{0};
+    uint16_t _flow_quality_sum{0};
+    uint8_t  _flow_sample_counter{0};
 
-	/**
+    /**
 	* Initialise the automatic measurement state machine and start it.
 	*
 	* @note This function is called at open and error time.  It might make sense
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
-	void start();
+    void start();
 
-	/**
+    /**
 	* Stop the automatic measurement state machine.
 	*/
-	void stop();
+    void stop();
 
 
-	int readRegister(unsigned reg, uint8_t *data, unsigned count);
-	int writeRegister(unsigned reg, uint8_t data);
+    int readRegister(unsigned reg, uint8_t *data, unsigned count);
+    int writeRegister(unsigned reg, uint8_t data);
 
-	int sensorInit();
-	int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual);
+    int sensorInit();
+    int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual);
 };
diff --git a/apps/peripheral/sensor/optical_flow/pmw3901/parameters.c b/apps/peripheral/sensor/optical_flow/pmw3901/parameters.c
index 74e9a48266..ed99d45cdf 100644
--- a/apps/peripheral/sensor/optical_flow/pmw3901/parameters.c
+++ b/apps/peripheral/sensor/optical_flow/pmw3901/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * PMW3901 Optical Flow
diff --git a/apps/peripheral/sensor/optical_flow/pmw3901/pmw3901_main.cpp b/apps/peripheral/sensor/optical_flow/pmw3901/pmw3901_main.cpp
index 6b87dfd0ce..e1f4bb30f0 100644
--- a/apps/peripheral/sensor/optical_flow/pmw3901/pmw3901_main.cpp
+++ b/apps/peripheral/sensor/optical_flow/pmw3901/pmw3901_main.cpp
@@ -1,89 +1,62 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2018-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "PMW3901.hpp"
 #include <px4_platform_common/module.h>
 
 extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]);
 
-void
-PMW3901::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("pmw3901", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void PMW3901::print_usage() {
+    PRINT_MODULE_USAGE_NAME("pmw3901", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+    PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-int
-pmw3901_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = PMW3901;
-	BusCLIArguments cli{false, true};
-	cli.spi_mode = SPIDEV_MODE0;
-	cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED;
+int pmw3901_main(int argc, char *argv[]) {
+    int ch;
+    using ThisDriver = PMW3901;
+    BusCLIArguments cli{false, true};
+    cli.spi_mode              = SPIDEV_MODE0;
+    cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED;
 
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
+    while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
+        switch (ch) {
+        case 'R':
+            cli.rotation = (enum Rotation)atoi(cli.optArg());
+            break;
+        }
+    }
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PMW3901);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PMW3901);
 
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
+    if (!strcmp(verb, "start")) {
+        return ThisDriver::module_start(cli, iterator);
+    }
 
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
+    if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
+    }
 
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/optical_flow/px4flow/i2c_frame.h b/apps/peripheral/sensor/optical_flow/px4flow/i2c_frame.h
index f0f05a93bd..bae6396acf 100644
--- a/apps/peripheral/sensor/optical_flow/px4flow/i2c_frame.h
+++ b/apps/peripheral/sensor/optical_flow/px4flow/i2c_frame.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2013-2015 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file i2c_frame.h
@@ -45,36 +22,35 @@
 
 #include <inttypes.h>
 
-typedef  struct i2c_frame {
-	uint16_t frame_count;       /**< counts created I2C frames [#frames] */
-	int16_t pixel_flow_x_sum;   /**< latest x flow measurement in pixels*10 [pixels] */
-	int16_t pixel_flow_y_sum;   /**< latest y flow measurement in pixels*10 [pixels] */
-	int16_t flow_comp_m_x;      /**< x velocity*1000 [meters/sec] */
-	int16_t flow_comp_m_y;      /**< y velocity*1000 [meters/sec] */
-	int16_t qual;               /**< Optical flow quality / confidence [0: bad, 255: maximum quality] */
-	int16_t gyro_x_rate;        /**< latest gyro x rate [rad/sec] */
-	int16_t gyro_y_rate;        /**< latest gyro y rate [rad/sec] */
-	int16_t gyro_z_rate;        /**< latest gyro z rate [rad/sec] */
-	uint8_t gyro_range;         /**< gyro range [0 .. 7] equals [50 deg/sec .. 2000 deg/sec] */
-	uint8_t sonar_timestamp;    /**< time since last sonar update [milliseconds] */
-	int16_t ground_distance;    /**< Ground distance in meters*1000 [meters]. Positive value: distance known. Negative value: Unknown distance */
+typedef struct i2c_frame {
+    uint16_t frame_count;      /**< counts created I2C frames [#frames] */
+    int16_t  pixel_flow_x_sum; /**< latest x flow measurement in pixels*10 [pixels] */
+    int16_t  pixel_flow_y_sum; /**< latest y flow measurement in pixels*10 [pixels] */
+    int16_t  flow_comp_m_x;    /**< x velocity*1000 [meters/sec] */
+    int16_t  flow_comp_m_y;    /**< y velocity*1000 [meters/sec] */
+    int16_t  qual;             /**< Optical flow quality / confidence [0: bad, 255: maximum quality] */
+    int16_t  gyro_x_rate;      /**< latest gyro x rate [rad/sec] */
+    int16_t  gyro_y_rate;      /**< latest gyro y rate [rad/sec] */
+    int16_t  gyro_z_rate;      /**< latest gyro z rate [rad/sec] */
+    uint8_t  gyro_range;       /**< gyro range [0 .. 7] equals [50 deg/sec .. 2000 deg/sec] */
+    uint8_t  sonar_timestamp;  /**< time since last sonar update [milliseconds] */
+    int16_t  ground_distance;  /**< Ground distance in meters*1000 [meters]. Positive value: distance known. Negative value: Unknown distance */
 } i2c_frame;
 
 #define I2C_FRAME_SIZE (sizeof(i2c_frame))
 
-
 typedef struct i2c_integral_frame {
-	uint16_t frame_count_since_last_readout; /**< number of flow measurements since last I2C readout [#frames] */
-	int16_t pixel_flow_x_integral;           /**< accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000] */
-	int16_t pixel_flow_y_integral;           /**< accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000] */
-	int16_t gyro_x_rate_integral;            /**< accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000] */
-	int16_t gyro_y_rate_integral;            /**< accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000] */
-	int16_t gyro_z_rate_integral;            /**< accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000] */
-	uint32_t integration_timespan;           /**< accumulation timespan in microseconds since last I2C readout [microseconds] */
-	uint32_t sonar_timestamp;                /**< time since last sonar update [microseconds] */
-	uint16_t ground_distance;                /**< Ground distance in meters*1000 [meters*1000] */
-	int16_t gyro_temperature;                /**< Temperature * 100 in centi-degrees Celsius [degcelsius*100] */
-	uint8_t qual;                            /**< averaged quality of accumulated flow values [0:bad quality;255: max quality] */
+    uint16_t frame_count_since_last_readout; /**< number of flow measurements since last I2C readout [#frames] */
+    int16_t  pixel_flow_x_integral;          /**< accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000] */
+    int16_t  pixel_flow_y_integral;          /**< accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000] */
+    int16_t  gyro_x_rate_integral;           /**< accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000] */
+    int16_t  gyro_y_rate_integral;           /**< accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000] */
+    int16_t  gyro_z_rate_integral;           /**< accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000] */
+    uint32_t integration_timespan;           /**< accumulation timespan in microseconds since last I2C readout [microseconds] */
+    uint32_t sonar_timestamp;                /**< time since last sonar update [microseconds] */
+    uint16_t ground_distance;                /**< Ground distance in meters*1000 [meters*1000] */
+    int16_t  gyro_temperature;               /**< Temperature * 100 in centi-degrees Celsius [degcelsius*100] */
+    uint8_t  qual;                           /**< averaged quality of accumulated flow values [0:bad quality;255: max quality] */
 } i2c_integral_frame;
 
 #define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
diff --git a/apps/peripheral/sensor/optical_flow/px4flow/parameters.c b/apps/peripheral/sensor/optical_flow/px4flow/parameters.c
index 485dd08416..11c0ceea44 100644
--- a/apps/peripheral/sensor/optical_flow/px4flow/parameters.c
+++ b/apps/peripheral/sensor/optical_flow/px4flow/parameters.c
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * PX4 Flow Optical Flow
diff --git a/apps/peripheral/sensor/optical_flow/px4flow/px4flow.cpp b/apps/peripheral/sensor/optical_flow/px4flow/px4flow.cpp
index 5002a5106d..23efc291dd 100644
--- a/apps/peripheral/sensor/optical_flow/px4flow/px4flow.cpp
+++ b/apps/peripheral/sensor/optical_flow/px4flow/px4flow.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file px4flow.cpp
@@ -51,299 +28,278 @@
 #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
-#define I2C_FLOW_ADDRESS_MIN        0x42	///< 7-bit address.
-#define I2C_FLOW_ADDRESS_MAX        0x49	///< 7-bit address.
+#define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
+#define I2C_FLOW_ADDRESS_MIN     0x42 ///< 7-bit address.
+#define I2C_FLOW_ADDRESS_MAX     0x49 ///< 7-bit address.
 
 /* PX4FLOW Registers addresses */
-#define PX4FLOW_REG			0x16	///< Measure Register 22
+#define PX4FLOW_REG                         0x16    ///< Measure Register 22
 
-#define PX4FLOW_CONVERSION_INTERVAL_DEFAULT 100000	///< in microseconds! = 10Hz
-#define PX4FLOW_CONVERSION_INTERVAL_MIN      10000	///< in microseconds! = 100 Hz
-#define PX4FLOW_CONVERSION_INTERVAL_MAX    1000000	///< in microseconds! = 1 Hz
+#define PX4FLOW_CONVERSION_INTERVAL_DEFAULT 100000  ///< in microseconds! = 10Hz
+#define PX4FLOW_CONVERSION_INTERVAL_MIN     10000   ///< in microseconds! = 100 Hz
+#define PX4FLOW_CONVERSION_INTERVAL_MAX     1000000 ///< in microseconds! = 1 Hz
 
-#define PX4FLOW_I2C_MAX_BUS_SPEED	400000	///< 400 KHz maximum speed
+#define PX4FLOW_I2C_MAX_BUS_SPEED           400000  ///< 400 KHz maximum speed
 
-#define PX4FLOW_MAX_DISTANCE 5.0f
-#define PX4FLOW_MIN_DISTANCE 0.3f
+#define PX4FLOW_MAX_DISTANCE                5.0f
+#define PX4FLOW_MIN_DISTANCE                0.3f
 
 #include "i2c_frame.h"
 
-class PX4FLOW: public device::I2C, public I2CSPIDriver<PX4FLOW>
-{
+class PX4FLOW : public device::I2C, public I2CSPIDriver<PX4FLOW> {
 public:
-	PX4FLOW(const I2CSPIDriverConfig &config);
-	virtual ~PX4FLOW();
+    PX4FLOW(const I2CSPIDriverConfig &config);
+    virtual ~PX4FLOW();
 
-	static void print_usage();
+    static void print_usage();
 
-	int init() override;
+    int init() override;
 
-	void print_status();
+    void print_status();
 
-	/**
+    /**
 	 * Perform a poll cycle; collect from the previous measurement
 	 * and start a new one.
 	 */
-	void				RunImpl();
+    void RunImpl();
 
 private:
-	int			probe() override;
+    int probe() override;
 
-	bool				_sensor_ok{false};
-	bool				_collect_phase{false};
+    bool _sensor_ok{false};
+    bool _collect_phase{false};
 
-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_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;
+    perf_counter_t _sample_perf;
+    perf_counter_t _comms_errors;
 
-	i2c_frame _frame;
+    i2c_frame _frame;
 
-	/**
+    /**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
 	 *
 	 * @param address	The I2C bus address to probe.
 	 * @return		True if the device is present.
 	 */
-	int					probe_address(uint8_t address);
+    int probe_address(uint8_t address);
 
-	/**
+    /**
 	 * Initialise the automatic measurement state machine and start it.
 	 *
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
 	 */
-	void				start();
-
-	int					measure();
-	int					collect();
+    void start();
 
+    int measure();
+    int collect();
 };
 
 PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config),
-	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
-{
+    I2C(config),
+    I2CSPIDriver(config),
+    _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": read")),
+    _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME ": com_err")) {
 }
 
-PX4FLOW::~PX4FLOW()
-{
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
+PX4FLOW::~PX4FLOW() {
+    perf_free(_sample_perf);
+    perf_free(_comms_errors);
 }
 
-int
-PX4FLOW::init()
-{
-	int ret = PX4_ERROR;
+int PX4FLOW::init() {
+    int ret = PX4_ERROR;
 
-	/* do I2C init (and probe) first */
-	if (I2C::init() != OK) {
-		return ret;
-	}
+    /* do I2C init (and probe) first */
+    if (I2C::init() != OK) {
+        return ret;
+    }
 
-	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
+    ret = OK;
+    /* sensor is ok, but we don't really know if it is within range */
+    _sensor_ok = true;
 
-	start();
+    start();
 
-	return ret;
+    return ret;
 }
 
-int
-PX4FLOW::probe()
-{
-	uint8_t val[I2C_FRAME_SIZE] {};
-
-	// to be sure this is not a ll40ls Lidar (which can also be on
-	// 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
-	// 0. The ll40ls gives an error for that, whereas the flow
-	// happily returns some data
-	if (transfer(nullptr, 0, &val[0], 22) != OK) {
-		return -EIO;
-	}
-
-	// that worked, so start a measurement cycle
-	return measure();
+int PX4FLOW::probe() {
+    uint8_t val[I2C_FRAME_SIZE]{};
+
+    // to be sure this is not a ll40ls Lidar (which can also be on
+    // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
+    // 0. The ll40ls gives an error for that, whereas the flow
+    // happily returns some data
+    if (transfer(nullptr, 0, &val[0], 22) != OK) {
+        return -EIO;
+    }
+
+    // that worked, so start a measurement cycle
+    return measure();
 }
 
-int
-PX4FLOW::measure()
-{
-	/*
+int PX4FLOW::measure() {
+    /*
 	 * Send the command to begin a measurement.
 	 */
-	uint8_t cmd = PX4FLOW_REG;
-	int ret = transfer(&cmd, 1, nullptr, 0);
+    uint8_t cmd = PX4FLOW_REG;
+    int     ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (OK != ret) {
-		perf_count(_comms_errors);
-		DEVICE_DEBUG("i2c::transfer returned %d", ret);
-		return ret;
-	}
+    if (OK != ret) {
+        perf_count(_comms_errors);
+        DEVICE_DEBUG("i2c::transfer returned %d", ret);
+        return ret;
+    }
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-int
-PX4FLOW::collect()
-{
-	int ret = -EIO;
+int PX4FLOW::collect() {
+    int ret = -EIO;
 
-	/* read from the sensor */
-	uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { };
+    /* read from the sensor */
+    uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = {};
 
-	perf_begin(_sample_perf);
+    perf_begin(_sample_perf);
 
-	if (PX4FLOW_REG == 0x00) {
-		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
-	}
+    if (PX4FLOW_REG == 0x00) {
+        ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
+    }
 
-	if (PX4FLOW_REG == 0x16) {
-		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
-	}
+    if (PX4FLOW_REG == 0x16) {
+        ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
+    }
 
-	if (ret < 0) {
-		DEVICE_DEBUG("error reading from sensor: %d", ret);
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
+    if (ret < 0) {
+        DEVICE_DEBUG("error reading from sensor: %d", ret);
+        perf_count(_comms_errors);
+        perf_end(_sample_perf);
+        return ret;
+    }
 
-	i2c_integral_frame _frame_integral{};
+    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);
-	}
+    if (PX4FLOW_REG == 0) {
+        memcpy(&_frame, val, I2C_FRAME_SIZE);
+        memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
+    }
 
-	if (PX4FLOW_REG == 0x16) {
-		memcpy(&_frame_integral, val, I2C_INTEGRAL_FRAME_SIZE);
-	}
+    if (PX4FLOW_REG == 0x16) {
+        memcpy(&_frame_integral, val, I2C_INTEGRAL_FRAME_SIZE);
+    }
 
 
-	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();
+    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{};
+    sensor_optical_flow_s report{};
 
-	report.timestamp_sample = hrt_absolute_time();
-	report.device_id = device_id.devid;
+    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.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.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.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.integration_timespan_us = _frame_integral.integration_timespan; // microseconds
 
-	report.quality = _frame_integral.qual; // 0:bad ; 255 max quality
+    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.max_flow_rate       = 2.5f;
+    report.min_ground_distance = PX4FLOW_MIN_DISTANCE;
+    report.max_ground_distance = PX4FLOW_MAX_DISTANCE;
 
-	report.timestamp = hrt_absolute_time();
-	_sensor_optical_flow_pub.publish(report);
+    report.timestamp = hrt_absolute_time();
+    _sensor_optical_flow_pub.publish(report);
 
-	perf_end(_sample_perf);
+    perf_end(_sample_perf);
 
-	return PX4_OK;
+    return PX4_OK;
 }
 
-void
-PX4FLOW::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
+void PX4FLOW::start() {
+    /* reset the report ring and state machine */
+    _collect_phase = false;
 
-	/* schedule a cycle to start things */
-	ScheduleNow();
+    /* schedule a cycle to start things */
+    ScheduleNow();
 }
 
-void
-PX4FLOW::RunImpl()
-{
-	if (OK != measure()) {
-		DEVICE_DEBUG("measure error");
-	}
-
-	/* perform collection */
-	if (OK != collect()) {
-		DEVICE_DEBUG("collection error");
-		/* restart the measurement state machine */
-		start();
-		return;
-	}
-
-	ScheduleDelayed(PX4FLOW_CONVERSION_INTERVAL_DEFAULT);
+void PX4FLOW::RunImpl() {
+    if (OK != measure()) {
+        DEVICE_DEBUG("measure error");
+    }
+
+    /* perform collection */
+    if (OK != collect()) {
+        DEVICE_DEBUG("collection error");
+        /* restart the measurement state machine */
+        start();
+        return;
+    }
+
+    ScheduleDelayed(PX4FLOW_CONVERSION_INTERVAL_DEFAULT);
 }
 
-void
-PX4FLOW::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+void PX4FLOW::print_status() {
+    I2CSPIDriverBase::print_status();
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
 }
 
-void
-PX4FLOW::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("px4flow", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+void PX4FLOW::print_usage() {
+    PRINT_MODULE_USAGE_NAME("px4flow", "driver");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+    PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
-extern "C" __EXPORT int px4flow_main(int argc, char *argv[])
-{
-	using ThisDriver = PX4FLOW;
-	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED;
-	cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT;
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[]) {
+    using ThisDriver = PX4FLOW;
+    BusCLIArguments cli{true, false};
+    cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED;
+    cli.i2c_address           = I2C_FLOW_ADDRESS_DEFAULT;
 
-	const char *verb = cli.optArg();
+    const char *verb = cli.optArg();
 
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
+    if (!verb) {
+        ThisDriver::print_usage();
+        return -1;
+    }
 
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PX4FLOW);
+    BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PX4FLOW);
 
-	if (!strcmp(verb, "start")) {
-		// px4flow can require more time to fully start and be accessible
-		static constexpr uint64_t STARTUP_MIN_TIME_US = 6'000'000;
-		const hrt_abstime time_now_us = hrt_absolute_time();
+    if (!strcmp(verb, "start")) {
+        // px4flow can require more time to fully start and be accessible
+        static constexpr uint64_t STARTUP_MIN_TIME_US = 6'000'000;
+        const hrt_abstime         time_now_us         = hrt_absolute_time();
 
-		if (time_now_us < STARTUP_MIN_TIME_US) {
-			px4_usleep(STARTUP_MIN_TIME_US - time_now_us);
-		}
+        if (time_now_us < STARTUP_MIN_TIME_US) {
+            px4_usleep(STARTUP_MIN_TIME_US - time_now_us);
+        }
 
-		return ThisDriver::module_start(cli, iterator);
+        return ThisDriver::module_start(cli, iterator);
 
-	} else if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
+    } else if (!strcmp(verb, "stop")) {
+        return ThisDriver::module_stop(iterator);
 
-	} else if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
+    } else if (!strcmp(verb, "status")) {
+        return ThisDriver::module_status(iterator);
+    }
 
-	ThisDriver::print_usage();
-	return -1;
+    ThisDriver::print_usage();
+    return -1;
 }
diff --git a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow.cpp b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow.cpp
index 71110f0403..b155bfce13 100644
--- a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow.cpp
+++ b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   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
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @author Mohammed Kabir <mhkabir98@gmail.com>
@@ -39,7 +16,7 @@
 
 #include <termios.h>
 #ifdef __PX4_CYGWIN
-#include <asm/socket.h>
+#   include <asm/socket.h>
 #endif
 
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -57,371 +34,346 @@
 
 #include "thoneflow_parser.h"
 
-class Thoneflow : public px4::ScheduledWorkItem
-{
+class Thoneflow : public px4::ScheduledWorkItem {
 public:
-	Thoneflow(const char *port);
-	virtual ~Thoneflow();
+    Thoneflow(const char *port);
+    virtual ~Thoneflow();
 
-	virtual int init();
+    virtual int init();
 
-	/**
+    /**
 	* Diagnostics - print some basic information about the driver.
 	*/
-	void print_info();
+    void print_info();
 
 private:
-	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};
+    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{0};
+    hrt_abstime _last_read{0};
 
-	uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+    uORB::PublicationMulti<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
-	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")};
+    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.
 	*/
-	void                start();
+    void start();
 
-	/**
+    /**
 	* Stop the automatic measurement state machine.
 	*/
-	void                stop();
+    void stop();
 
-	/**
+    /**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void                Run() override;
-	int             collect();
-
+    void Run() override;
+    int  collect();
 };
 
 Thoneflow::Thoneflow(const char *port) :
-	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port))
-{
-	/* store port name */
-	strncpy(_port, port, sizeof(_port) - 1);
+    ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) {
+    /* store port name */
+    strncpy(_port, port, sizeof(_port) - 1);
 
-	/* enforce null termination */
-	_port[sizeof(_port) - 1] = '\0';
+    /* enforce null termination */
+    _port[sizeof(_port) - 1] = '\0';
 }
 
-Thoneflow::~Thoneflow()
-{
-	stop();
+Thoneflow::~Thoneflow() {
+    stop();
 
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
+    perf_free(_sample_perf);
+    perf_free(_comms_errors);
 }
 
-int
-Thoneflow::init()
-{
-	int ret = PX4_OK;
+int Thoneflow::init() {
+    int ret = PX4_OK;
 
-	do { /* create a scope to handle exit conditions using break */
+    do { /* create a scope to handle exit conditions using break */
 
-		/* open fd */
-		_fd = ::open(_port, O_RDONLY | O_NOCTTY);
+        /* open fd */
+        _fd = ::open(_port, O_RDONLY | O_NOCTTY);
 
-		if (_fd < 0) {
-			PX4_ERR("Error opening fd");
-			return -1;
-		}
-
-		/* Baudrate 19200, 8 bits, no parity, 1 stop bit */
-		unsigned speed = B19200;
-
-		struct termios uart_config;
-
-		int termios_state;
+        if (_fd < 0) {
+            PX4_ERR("Error opening fd");
+            return -1;
+        }
 
-		tcgetattr(_fd, &uart_config);
+        /* Baudrate 19200, 8 bits, no parity, 1 stop bit */
+        unsigned speed = B19200;
 
-		/* clear ONLCR flag (which appends a CR for every LF) */
-		uart_config.c_oflag &= ~ONLCR;
+        struct termios uart_config;
 
-		/* set baud rate */
-		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-			PX4_ERR("CFG: %d ISPD", termios_state);
-			ret = PX4_ERROR;
-			break;
-		}
+        int termios_state;
 
-		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-			PX4_ERR("CFG: %d OSPD\n", termios_state);
-			ret = PX4_ERROR;
-			break;
-		}
+        tcgetattr(_fd, &uart_config);
 
-		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
-			PX4_ERR("baud %d ATTR", termios_state);
-			ret = PX4_ERROR;
-			break;
-		}
+        /* clear ONLCR flag (which appends a CR for every LF) */
+        uart_config.c_oflag &= ~ONLCR;
 
-		uart_config.c_cflag |= (CLOCAL | CREAD);    /* ignore modem controls */
-		uart_config.c_cflag &= ~CSIZE;
-		uart_config.c_cflag |= CS8;         /* 8-bit characters */
-		uart_config.c_cflag &= ~PARENB;     /* no parity bit */
-		uart_config.c_cflag &= ~CSTOPB;     /* only need 1 stop bit */
-		uart_config.c_cflag &= ~CRTSCTS;    /* no hardware flowcontrol */
+        /* set baud rate */
+        if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+            PX4_ERR("CFG: %d ISPD", termios_state);
+            ret = PX4_ERROR;
+            break;
+        }
 
-		/* setup for non-canonical mode */
-		uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
-		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
-		uart_config.c_oflag &= ~OPOST;
+        if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+            PX4_ERR("CFG: %d OSPD\n", termios_state);
+            ret = PX4_ERROR;
+            break;
+        }
 
-		/* fetch bytes as they become available */
-		uart_config.c_cc[VMIN] = 1;
-		uart_config.c_cc[VTIME] = 1;
+        if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+            PX4_ERR("baud %d ATTR", termios_state);
+            ret = PX4_ERROR;
+            break;
+        }
 
-		if (_fd < 0) {
-			PX4_ERR("FAIL: flow fd");
-			ret = PX4_ERROR;
-			break;
-		}
-	} while (0);
+        uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
+        uart_config.c_cflag &= ~CSIZE;
+        uart_config.c_cflag |= CS8;              /* 8-bit characters */
+        uart_config.c_cflag &= ~PARENB;          /* no parity bit */
+        uart_config.c_cflag &= ~CSTOPB;          /* only need 1 stop bit */
+        uart_config.c_cflag &= ~CRTSCTS;         /* no hardware flowcontrol */
+
+        /* setup for non-canonical mode */
+        uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+        uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+        uart_config.c_oflag &= ~OPOST;
+
+        /* fetch bytes as they become available */
+        uart_config.c_cc[VMIN]  = 1;
+        uart_config.c_cc[VTIME] = 1;
+
+        if (_fd < 0) {
+            PX4_ERR("FAIL: flow fd");
+            ret = PX4_ERROR;
+            break;
+        }
+    } while (0);
 
-	/* Close the fd */
-	::close(_fd);
-	_fd = -1;
+    /* Close the fd */
+    ::close(_fd);
+    _fd = -1;
 
-	/* Start measurement */
-	start();
+    /* Start measurement */
+    start();
 
-	return ret;
+    return ret;
 }
 
-int
-Thoneflow::collect()
-{
-	perf_begin(_sample_perf);
+int Thoneflow::collect() {
+    perf_begin(_sample_perf);
 
-	/* clear buffer if last read was too long ago */
-	int64_t read_elapsed = hrt_elapsed_time(&_last_read);
+    /* clear buffer if last read was too long ago */
+    int64_t read_elapsed = hrt_elapsed_time(&_last_read);
 
-	/* the buffer for read chars is the packet size */
-	char readbuf[10];
-	const unsigned readlen = 9;
+    /* the buffer for read chars is the packet size */
+    char           readbuf[10];
+    const unsigned readlen = 9;
 
-	int ret = 0;
+    int ret = 0;
 
-	/* Check the number of bytes available in the buffer*/
-	int bytes_available = 0;
-	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
+    /* Check the number of bytes available in the buffer*/
+    int bytes_available = 0;
+    ::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
 
-	if (!bytes_available) {
-		return -EAGAIN;
-	}
-
-	bool valid = false;
+    if (!bytes_available) {
+        return -EAGAIN;
+    }
 
-	do {
-		/* Read from UART buffer) */
-		ret = ::read(_fd, &readbuf[0], readlen);
+    bool valid = false;
 
-		if (ret < 0) {
-			PX4_ERR("read err: %d", ret);
-			perf_count(_comms_errors);
-			perf_end(_sample_perf);
+    do {
+        /* Read from UART buffer) */
+        ret = ::read(_fd, &readbuf[0], readlen);
 
-			/* only throw an error if we time out */
-			if (read_elapsed > (_cycle_interval * 2)) {
-				/* flush anything in RX buffer */
-				tcflush(_fd, TCIFLUSH);
-				return ret;
+        if (ret < 0) {
+            PX4_ERR("read err: %d", ret);
+            perf_count(_comms_errors);
+            perf_end(_sample_perf);
 
-			} else {
-				return -EAGAIN;
-			}
-		}
+            /* only throw an error if we time out */
+            if (read_elapsed > (_cycle_interval * 2)) {
+                /* flush anything in RX buffer */
+                tcflush(_fd, TCIFLUSH);
+                return ret;
 
-		_last_read = hrt_absolute_time();
+            } else {
+                return -EAGAIN;
+            }
+        }
 
-		// publish sensor_optical_flow
-		sensor_optical_flow_s report{};
-		report.timestamp_sample = hrt_absolute_time();
+        _last_read = 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);
-		}
+        // publish sensor_optical_flow
+        sensor_optical_flow_s report{};
+        report.timestamp_sample = hrt_absolute_time();
 
-		/* Publish most recent valid measurement */
-		if (valid) {
+        /* Parse each byte of read buffer */
+        for (int i = 0; i < ret; i++) {
+            valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report);
+        }
 
-			report.device_id = 0; // TODO get_device_id();
-			report.integration_timespan_us = 10526; // microseconds
+        /* Publish most recent valid measurement */
+        if (valid) {
+            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[0], report.pixel_flow[1], zeroval);
+            /* Rotate measurements from sensor frame to body frame */
+            float zeroval = 0.0f;
+            rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval);
 
-			// 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
+            // 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();
+            report.timestamp = hrt_absolute_time();
 
-			_sensor_optical_flow_pub.publish(report);
-		}
+            _sensor_optical_flow_pub.publish(report);
+        }
 
-		/* Bytes left to parse */
-		bytes_available -= ret;
+        /* Bytes left to parse */
+        bytes_available -= ret;
 
-	} while (bytes_available > 0);
+    } while (bytes_available > 0);
 
-	/* no valid measurement after parsing all available bytes, or partial packet parsed */
-	if (!valid || _parse_state != THONEFLOW_PARSE_STATE9_FOOTER) {
-		return -EAGAIN;
-	}
+    /* no valid measurement after parsing all available bytes, or partial packet parsed */
+    if (!valid || _parse_state != THONEFLOW_PARSE_STATE9_FOOTER) {
+        return -EAGAIN;
+    }
 
-	ret = OK;
+    ret = OK;
 
-	perf_end(_sample_perf);
-	return ret;
+    perf_end(_sample_perf);
+    return ret;
 }
 
-void
-Thoneflow::start()
-{
-	ScheduleNow();
+void Thoneflow::start() {
+    ScheduleNow();
 }
 
-void
-Thoneflow::stop()
-{
-	ScheduleClear();
+void Thoneflow::stop() {
+    ScheduleClear();
 }
 
-void
-Thoneflow::Run()
-{
-	/* fds initialized? */
-	if (_fd < 0) {
-		/* open fd */
-		_fd = ::open(_port, O_RDONLY | O_NOCTTY);
-	}
-
-	if (collect() == -EAGAIN) {
-		/* Reschedule earlier to grab the missing bits, time to transmit 9 bytes @ 19200 bps */
-		ScheduleDelayed(520 * 9);
-		return;
-	}
-
-	ScheduleDelayed(_cycle_interval);
+void Thoneflow::Run() {
+    /* fds initialized? */
+    if (_fd < 0) {
+        /* open fd */
+        _fd = ::open(_port, O_RDONLY | O_NOCTTY);
+    }
+
+    if (collect() == -EAGAIN) {
+        /* Reschedule earlier to grab the missing bits, time to transmit 9 bytes @ 19200 bps */
+        ScheduleDelayed(520 * 9);
+        return;
+    }
+
+    ScheduleDelayed(_cycle_interval);
 }
 
-void
-Thoneflow::print_info()
-{
-	PX4_INFO("Using port '%s'", _port);
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
+void Thoneflow::print_info() {
+    PX4_INFO("Using port '%s'", _port);
+    perf_print_counter(_sample_perf);
+    perf_print_counter(_comms_errors);
 }
 
 /**
  * Local functions in support of the shell command.
  */
-namespace thoneflow
-{
+namespace thoneflow {
 
-Thoneflow   *g_dev;
+Thoneflow *g_dev;
 
-int start(const char *port);
-int stop();
-int info();
+int  start(const char *port);
+int  stop();
+int  info();
 void usage();
 
 /**
  * Start the driver.
  */
-int
-start(const char *port)
-{
-	if (g_dev != nullptr) {
-		PX4_ERR("already started");
-		return 1;
-	}
+int start(const char *port) {
+    if (g_dev != nullptr) {
+        PX4_ERR("already started");
+        return 1;
+    }
 
-	/* create the driver */
-	g_dev = new Thoneflow(port);
+    /* create the driver */
+    g_dev = new Thoneflow(port);
 
-	if (g_dev == nullptr) {
-		goto fail;
-	}
+    if (g_dev == nullptr) {
+        goto fail;
+    }
 
-	if (OK != g_dev->init()) {
-		goto fail;
-	}
+    if (OK != g_dev->init()) {
+        goto fail;
+    }
 
-	return 0;
+    return 0;
 
 fail:
 
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
+    if (g_dev != nullptr) {
+        delete g_dev;
+        g_dev = nullptr;
+    }
 
-	PX4_ERR("driver start failed");
-	return 1;
+    PX4_ERR("driver start failed");
+    return 1;
 }
 
 /**
  * Stop the driver
  */
-int stop()
-{
-	if (g_dev != nullptr) {
-		PX4_INFO("stopping driver");
-		delete g_dev;
-		g_dev = nullptr;
-		PX4_INFO("driver stopped");
-
-	} else {
-		PX4_ERR("driver not running");
-		return 1;
-	}
-
-	return 0;
+int stop() {
+    if (g_dev != nullptr) {
+        PX4_INFO("stopping driver");
+        delete g_dev;
+        g_dev = nullptr;
+        PX4_INFO("driver stopped");
+
+    } else {
+        PX4_ERR("driver not running");
+        return 1;
+    }
+
+    return 0;
 }
 
 /**
  * Print a little info about the driver.
  */
-int
-info()
-{
-	if (g_dev == nullptr) {
-		PX4_ERR("driver not running");
-		return 1;
-	}
+int info() {
+    if (g_dev == nullptr) {
+        PX4_ERR("driver not running");
+        return 1;
+    }
 
-	g_dev->print_info();
+    g_dev->print_info();
 
-	return 0;
+    return 0;
 }
 
 /**
  * Print a little info on how to use the driver.
  */
-void
-usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
+void usage() {
+    PRINT_MODULE_DESCRIPTION(
+        R"DESCR_STR(
 ### Description
 
 Serial bus driver for the ThoneFlow-3901U optical flow sensor.
@@ -446,14 +398,13 @@ Stop driver
     PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information");
 }
 
-} // namespace
+} // namespace thoneflow
 
-extern "C" __EXPORT int thoneflow_main(int argc, char *argv[])
-{
-    int ch;
+extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]) {
+    int         ch;
     const char *device_path = "";
-    int myoptind = 1;
-    const char *myoptarg = nullptr;
+    int         myoptind    = 1;
+    const char *myoptarg    = nullptr;
 
     while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
         switch (ch) {
diff --git a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.cpp b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.cpp
index 3534af002a..6f55cc9d2b 100644
--- a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.cpp
+++ b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @author Mohammed Kabir <mhkabir98@gmail.com>
@@ -44,130 +21,127 @@
 //#define THONEFLOW_DEBUG
 
 #ifdef THONEFLOW_DEBUG
-#include <stdio.h>
+#   include <stdio.h>
 
 const char *parser_state[] = {
-	"0_UNSYNC",
-	"1_HEADER",
-	"2_NBYTES",
-	"3_XM_L",
-	"4_XM_H",
-	"5_YM_L",
-	"6_YM_H",
-	"7_CHECKSUM",
-	"8_QUALITY",
-	"9_FOOTER"
-};
+    "0_UNSYNC",
+    "1_HEADER",
+    "2_NBYTES",
+    "3_XM_L",
+    "4_XM_H",
+    "5_YM_L",
+    "6_YM_H",
+    "7_CHECKSUM",
+    "8_QUALITY",
+    "9_FOOTER"};
 #endif
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     sensor_optical_flow_s *flow)
-{
-	bool parsed_packet = false;
-
-	switch (*state) {
-	case THONEFLOW_PARSE_STATE9_FOOTER:
-		if (c == 0xFE) {
-			*state = THONEFLOW_PARSE_STATE1_HEADER;
+                     sensor_optical_flow_s *flow) {
+    bool parsed_packet = false;
 
-		} else {
-			*state = THONEFLOW_PARSE_STATE0_UNSYNC;
-		}
+    switch (*state) {
+    case THONEFLOW_PARSE_STATE9_FOOTER:
+        if (c == 0xFE) {
+            *state = THONEFLOW_PARSE_STATE1_HEADER;
 
-		break;
+        } else {
+            *state = THONEFLOW_PARSE_STATE0_UNSYNC;
+        }
 
-	case THONEFLOW_PARSE_STATE0_UNSYNC:
-		if (c == 0xFE) {
-			*state = THONEFLOW_PARSE_STATE1_HEADER;
-		}
+        break;
 
-		break;
+    case THONEFLOW_PARSE_STATE0_UNSYNC:
+        if (c == 0xFE) {
+            *state = THONEFLOW_PARSE_STATE1_HEADER;
+        }
 
-	case THONEFLOW_PARSE_STATE1_HEADER:
-		if (c == 0x04) {
-			*state = THONEFLOW_PARSE_STATE2_NBYTES;
+        break;
 
-		} else {
-			*state = THONEFLOW_PARSE_STATE0_UNSYNC;
-		}
+    case THONEFLOW_PARSE_STATE1_HEADER:
+        if (c == 0x04) {
+            *state = THONEFLOW_PARSE_STATE2_NBYTES;
 
-		break;
+        } else {
+            *state = THONEFLOW_PARSE_STATE0_UNSYNC;
+        }
 
-	case THONEFLOW_PARSE_STATE2_NBYTES:
-		*state = THONEFLOW_PARSE_STATE3_XM_L;
-		parserbuf[*parserbuf_index] = c;
-		(*parserbuf_index)++;
+        break;
 
-		break;
+    case THONEFLOW_PARSE_STATE2_NBYTES:
+        *state                      = THONEFLOW_PARSE_STATE3_XM_L;
+        parserbuf[*parserbuf_index] = c;
+        (*parserbuf_index)++;
 
-	case THONEFLOW_PARSE_STATE3_XM_L:
-		*state = THONEFLOW_PARSE_STATE4_XM_H;
-		parserbuf[*parserbuf_index] = c;
-		(*parserbuf_index)++;
+        break;
 
-		break;
+    case THONEFLOW_PARSE_STATE3_XM_L:
+        *state                      = THONEFLOW_PARSE_STATE4_XM_H;
+        parserbuf[*parserbuf_index] = c;
+        (*parserbuf_index)++;
 
-	case THONEFLOW_PARSE_STATE4_XM_H:
-		*state = THONEFLOW_PARSE_STATE5_YM_L;
-		parserbuf[*parserbuf_index] = c;
-		(*parserbuf_index)++;
+        break;
 
-		break;
+    case THONEFLOW_PARSE_STATE4_XM_H:
+        *state                      = THONEFLOW_PARSE_STATE5_YM_L;
+        parserbuf[*parserbuf_index] = c;
+        (*parserbuf_index)++;
 
-	case THONEFLOW_PARSE_STATE5_YM_L:
-		*state = THONEFLOW_PARSE_STATE6_YM_H;
-		parserbuf[*parserbuf_index] = c;
-		(*parserbuf_index)++;
+        break;
 
-		break;
+    case THONEFLOW_PARSE_STATE5_YM_L:
+        *state                      = THONEFLOW_PARSE_STATE6_YM_H;
+        parserbuf[*parserbuf_index] = c;
+        (*parserbuf_index)++;
 
-	case THONEFLOW_PARSE_STATE6_YM_H: {
-			unsigned char cksm = 0;
+        break;
 
-			// Calculate checksum over motion values
-			for (int i = 0; i < 4; i++) {
-				cksm += parserbuf[i];
-			}
+    case THONEFLOW_PARSE_STATE6_YM_H: {
+        unsigned char cksm = 0;
 
-			if (c == cksm) {
-				// 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[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;
+        // Calculate checksum over motion values
+        for (int i = 0; i < 4; i++) {
+            cksm += parserbuf[i];
+        }
 
-			} else {
-				*state = THONEFLOW_PARSE_STATE0_UNSYNC;
-			}
+        if (c == cksm) {
+            // 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[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;
 
-			*parserbuf_index = 0;
-		}
+        } else {
+            *state = THONEFLOW_PARSE_STATE0_UNSYNC;
+        }
 
-		break;
+        *parserbuf_index = 0;
+    }
 
-	case THONEFLOW_PARSE_STATE7_CHECKSUM:
-		*state = THONEFLOW_PARSE_STATE8_QUALITY;
-		flow->quality = uint8_t(c);
+    break;
 
-		break;
+    case THONEFLOW_PARSE_STATE7_CHECKSUM:
+        *state        = THONEFLOW_PARSE_STATE8_QUALITY;
+        flow->quality = uint8_t(c);
 
-	case THONEFLOW_PARSE_STATE8_QUALITY:
-		if (c == 0xAA) {
-			*state = THONEFLOW_PARSE_STATE9_FOOTER;
-			parsed_packet = true;
+        break;
 
-		} else {
-			*state = THONEFLOW_PARSE_STATE0_UNSYNC;
-		}
+    case THONEFLOW_PARSE_STATE8_QUALITY:
+        if (c == 0xAA) {
+            *state        = THONEFLOW_PARSE_STATE9_FOOTER;
+            parsed_packet = true;
 
-		break;
+        } else {
+            *state = THONEFLOW_PARSE_STATE0_UNSYNC;
+        }
 
-	}
+        break;
+    }
 
 #ifdef THONEFLOW_DEBUG
-	printf("state: THONEFLOW_PARSE_STATE%s, got char: %#02x\n", parser_state[*state], c);
+    printf("state: THONEFLOW_PARSE_STATE%s, got char: %#02x\n", parser_state[*state], c);
 #endif
 
-	return parsed_packet;
+    return parsed_packet;
 }
diff --git a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.h b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.h
index d63906fdc6..2e98f57030 100644
--- a/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.h
+++ b/apps/peripheral/sensor/optical_flow/thoneflow/thoneflow_parser.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -49,17 +26,17 @@
 // 9) Footer (0xAA)
 
 enum THONEFLOW_PARSE_STATE {
-	THONEFLOW_PARSE_STATE0_UNSYNC = 0,
-	THONEFLOW_PARSE_STATE1_HEADER,
-	THONEFLOW_PARSE_STATE2_NBYTES,
-	THONEFLOW_PARSE_STATE3_XM_L,
-	THONEFLOW_PARSE_STATE4_XM_H,
-	THONEFLOW_PARSE_STATE5_YM_L,
-	THONEFLOW_PARSE_STATE6_YM_H,
-	THONEFLOW_PARSE_STATE7_CHECKSUM,
-	THONEFLOW_PARSE_STATE8_QUALITY,
-	THONEFLOW_PARSE_STATE9_FOOTER
+    THONEFLOW_PARSE_STATE0_UNSYNC = 0,
+    THONEFLOW_PARSE_STATE1_HEADER,
+    THONEFLOW_PARSE_STATE2_NBYTES,
+    THONEFLOW_PARSE_STATE3_XM_L,
+    THONEFLOW_PARSE_STATE4_XM_H,
+    THONEFLOW_PARSE_STATE5_YM_L,
+    THONEFLOW_PARSE_STATE6_YM_H,
+    THONEFLOW_PARSE_STATE7_CHECKSUM,
+    THONEFLOW_PARSE_STATE8_QUALITY,
+    THONEFLOW_PARSE_STATE9_FOOTER
 };
 
 bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state,
-		     sensor_optical_flow_s *report);
+                     sensor_optical_flow_s *report);
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp
index 12fc861400..c15997bf18 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp
@@ -1,36 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp
index 671d60286e..6d79dcfe65 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp
@@ -1,37 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp
index 32a0cbc5a7..c1f72e42bb 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp
@@ -1,36 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp
index 97230ea3ea..e4b04f9629 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp
@@ -1,37 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <uavcan_nuttx/thread.hpp>
 #include <uavcan_nuttx/socketcan.hpp>
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp
index d627d2b728..04b71ca6be 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp
@@ -1,44 +1,12 @@
-/*
- * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *
- *
- */
-
-/****************************************************************************
- *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <uavcan_nuttx/socketcan.hpp>
 #include <uavcan_nuttx/clock.hpp>
diff --git a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp
index 1060c611f2..ad65730037 100644
--- a/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp
+++ b/apps/telemetry/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp
@@ -1,37 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
- *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
- *
- * 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <uavcan_nuttx/thread.hpp>
 #include <uavcan_nuttx/socketcan.hpp>
diff --git a/bsps/sitl/qemu/rtconfig.h b/bsps/sitl/qemu/rtconfig.h
index 161fb0f9cc..16655062d0 100644
--- a/bsps/sitl/qemu/rtconfig.h
+++ b/bsps/sitl/qemu/rtconfig.h
@@ -25,7 +25,6 @@
 #define RT_USING_DEBUG
 #define RT_DEBUGING_COLOR
 #define RT_DEBUGING_CONTEXT
-#define RT_DEBUGING_INIT
 
 /* Inter-Thread communication */
 
@@ -601,6 +600,35 @@
 
 /* Nextpilot Peripheral Config */
 
+/* actuator */
+
+/* end of actuator */
+
+/* battery */
+
+/* end of battery */
+
+/* notification */
+
+/* end of notification */
+
+/* payload */
+
+/* end of payload */
+
+/* radio control */
+
+/* end of radio control */
+
+/* sensor */
+
+
+/* Optical flow */
+
+/* end of Optical flow */
+/* end of sensor */
+/* end of Nextpilot Peripheral Config */
+
 /* Nextpilot Simulation Config */
 
 #define SIM_USING_BATTERY
@@ -628,6 +656,10 @@
 #define MAVLINK_USING_PARAM
 #define MAVLINK_USING_MISSION
 /* end of Nextpilot Telemetry Config */
+
+/* NextPilot UnitTest Config */
+
+/* end of NextPilot UnitTest Config */
 /* end of Nextpilot Firmware Config */
 
 /* Nextpilot Packages Config */
@@ -638,23 +670,25 @@
 #define PKG_USING_GETOPT
 #define PKG_USING_HRTIMER
 #define PKG_USING_HRTIMER_V3
-#define HRT_USING_OSTICK
+#define HRT_USING_OTHER
 #define HRT_USING_OSTIMER
 #define PKG_USING_LIBCRC
 #define PKG_USING_MATHLIB
 #define PKG_USING_MATRIX
-#define PARAM_DEFAULT_FILE_PATH "/param.bin"
 #define PKG_USING_PARAM
+#define PARAM_USING_GLOBAL_AUTOGEN
+#define PARAM_USING_STORAGE_FILE
+#define PARAM_DEFAULT_FILE_PATH "/param.bin"
 #define PARAM_USING_DEVICE_FILE
 #define PKG_USING_PERF
 #define PKG_USING_QUEUE
 #define PKG_USING_RING_BUFFER
 #define PKG_USING_SHUTDOWN
-#define PKG_USING_UORB
 
 /* ULog backend */
 
 /* end of ULog backend */
+#define PKG_USING_UORB
 #define PKG_USING_VAR_LEN_RINGBUFFER
 #define PKG_USING_VCONSOLE_V2
 #define PKG_USING_BOARD_VERSION
diff --git a/pkgs/board_identity/board_common.c b/pkgs/board_identity/board_common.c
index 495de6d3ca..6ac05873ff 100644
--- a/pkgs/board_identity/board_common.c
+++ b/pkgs/board_identity/board_common.c
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- *   Copyright (C) 2022 Technology Innovation Institute. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /****************************************************************************
  * Included Files
@@ -43,12 +20,10 @@
  * Public Functions
  ****************************************************************************/
 
-#if defined(GPIO_OTGFS_VBUS) && \
-    (defined(CONFIG_BUILD_FLAT) || !defined(__PX4_NUTTX))
+#if defined(GPIO_OTGFS_VBUS) && (defined(CONFIG_BUILD_FLAT) || !defined(__PX4_NUTTX))
 
 /* Default implementation for POSIX and flat NUTTX if the VBUS pin exists */
-int board_read_VBUS_state(void)
-{
-	return (px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1);
+int board_read_VBUS_state(void) {
+    return (px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1);
 }
 #endif
diff --git a/pkgs/console_buffer/console_buffer_usr.cpp b/pkgs/console_buffer/console_buffer_usr.cpp
index bcdf1bbb81..7c22866a43 100644
--- a/pkgs/console_buffer/console_buffer_usr.cpp
+++ b/pkgs/console_buffer/console_buffer_usr.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2021 Technology Innovation Institute. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 
 #include <console_buffer.h>
@@ -39,9 +16,9 @@
 #include <fcntl.h>
 
 #ifdef BOARD_ENABLE_CONSOLE_BUFFER
-#ifndef BOARD_CONSOLE_BUFFER_SIZE
-#define BOARD_CONSOLE_BUFFER_SIZE (1024 * 4) // default buffer size
-#endif
+#   ifndef BOARD_CONSOLE_BUFFER_SIZE
+#      define BOARD_CONSOLE_BUFFER_SIZE (1024 * 4) // default buffer size
+#   endif
 
 // TODO: User side implementation of px4_console_buffer
 
diff --git a/pkgs/device/cdev/test/cdevtest_example.cpp b/pkgs/device/cdev/test/cdevtest_example.cpp
index 7922ca1348..9b8be496c0 100644
--- a/pkgs/device/cdev/test/cdevtest_example.cpp
+++ b/pkgs/device/cdev/test/cdevtest_example.cpp
@@ -1,36 +1,13 @@
 
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2015 Mark Charlebois. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file cdevtest_example.cpp
diff --git a/pkgs/device/cdev/test/cdevtest_example.h b/pkgs/device/cdev/test/cdevtest_example.h
index dbe0144d2b..a121bc962f 100644
--- a/pkgs/device/cdev/test/cdevtest_example.h
+++ b/pkgs/device/cdev/test/cdevtest_example.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2015 Mark Charlebois. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file vcdevtest_example.h
diff --git a/pkgs/device/cdev/test/cdevtest_main.cpp b/pkgs/device/cdev/test/cdevtest_main.cpp
index 5a58299f61..93e77b9bce 100644
--- a/pkgs/device/cdev/test/cdevtest_main.cpp
+++ b/pkgs/device/cdev/test/cdevtest_main.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2015 Mark Charlebois. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file cdevtest_main.cpp
diff --git a/pkgs/device/cdev/test/cdevtest_start.cpp b/pkgs/device/cdev/test/cdevtest_start.cpp
index 41ca376e2c..b64c19ac77 100644
--- a/pkgs/device/cdev/test/cdevtest_start.cpp
+++ b/pkgs/device/cdev/test/cdevtest_start.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2015 Mark Charlebois. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file cdevtest_start.cpp
diff --git a/pkgs/device/device.hpp b/pkgs/device/device.hpp
index b50ed231e2..439ceba074 100644
--- a/pkgs/device/device.hpp
+++ b/pkgs/device/device.hpp
@@ -157,7 +157,7 @@ class Device : public rt_device {
 
     bool _registered{false};
 
-    // 驱动的名称，比如ist8310
+    // 驱动的名称，比如i2c1
     const char *_bus_name{nullptr};
     rt_device_t _bus_device{nullptr};
 };
diff --git a/pkgs/device/drv-device/i2c.h b/pkgs/device/drv-device/i2c.h
index f97c4b87a4..83acde37c9 100644
--- a/pkgs/device/drv-device/i2c.h
+++ b/pkgs/device/drv-device/i2c.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (C) 2015 Mark Charlebois. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 #pragma once
 
 #ifdef __PX4_NUTTX
diff --git a/pkgs/device/px4/i2c.cpp b/pkgs/device/px4/i2c.cpp
index 7f68a9cfcd..f9b7f70bbb 100644
--- a/pkgs/device/px4/i2c.cpp
+++ b/pkgs/device/px4/i2c.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <board_config.h>
 
@@ -37,69 +14,67 @@
 
 #if defined(CONFIG_I2C)
 
-#ifndef BOARD_OVERRIDE_I2C_BUS_EXTERNAL
-bool px4_i2c_bus_external(int bus)
-{
-	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
-		if (px4_i2c_buses[i].bus == bus) {
-			return px4_i2c_buses[i].is_external;
-		}
-	}
+#   ifndef BOARD_OVERRIDE_I2C_BUS_EXTERNAL
+bool px4_i2c_bus_external(int bus) {
+    for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
+        if (px4_i2c_buses[i].bus == bus) {
+            return px4_i2c_buses[i].is_external;
+        }
+    }
 
-	return true;
+    return true;
 }
-#endif // BOARD_OVERRIDE_I2C_BUS_EXTERNAL
-
-#ifndef BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL
-#include <drivers/device/Device.hpp>
-bool px4_i2c_device_external(const uint32_t device_id)
-{
-	device::Device::DeviceId dev_id{};
-	dev_id.devid = device_id;
-	return px4_i2c_bus_external(dev_id.devid_s.bus);
+#   endif // BOARD_OVERRIDE_I2C_BUS_EXTERNAL
+
+#   ifndef BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL
+#      include <drivers/device/Device.hpp>
+
+bool px4_i2c_device_external(const uint32_t device_id) {
+    device::Device::DeviceId dev_id{};
+    dev_id.devid = device_id;
+    return px4_i2c_bus_external(dev_id.devid_s.bus);
 }
-#endif // BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL
+#   endif // BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL
 
-bool I2CBusIterator::next()
-{
-	while (++_index < I2C_BUS_MAX_BUS_ITEMS && px4_i2c_buses[_index].bus != -1) {
-		const px4_i2c_bus_t &bus_data = px4_i2c_buses[_index];
+bool I2CBusIterator::next() {
+    while (++_index < I2C_BUS_MAX_BUS_ITEMS && px4_i2c_buses[_index].bus != -1) {
+        const px4_i2c_bus_t &bus_data = px4_i2c_buses[_index];
 
-		if (!board_has_bus(BOARD_I2C_BUS, bus_data.bus)) {
-			continue;
-		}
+        if (!board_has_bus(BOARD_I2C_BUS, bus_data.bus)) {
+            continue;
+        }
 
-		switch (_filter) {
-		case FilterType::All:
-			if (_bus == bus_data.bus || _bus == -1) {
-				return true;
-			}
+        switch (_filter) {
+        case FilterType::All:
+            if (_bus == bus_data.bus || _bus == -1) {
+                return true;
+            }
 
-			break;
+            break;
 
-		case FilterType::InternalBus:
-			if (!px4_i2c_bus_external(bus_data.bus)) {
-				if (_bus == bus_data.bus || _bus == -1) {
-					return true;
-				}
-			}
+        case FilterType::InternalBus:
+            if (!px4_i2c_bus_external(bus_data.bus)) {
+                if (_bus == bus_data.bus || _bus == -1) {
+                    return true;
+                }
+            }
 
-			break;
+            break;
 
-		case FilterType::ExternalBus:
-			if (px4_i2c_bus_external(bus_data.bus)) {
-				++_external_bus_counter;
+        case FilterType::ExternalBus:
+            if (px4_i2c_bus_external(bus_data.bus)) {
+                ++_external_bus_counter;
 
-				if (_bus == bus_data.bus || _bus == -1) {
-					return true;
-				}
-			}
+                if (_bus == bus_data.bus || _bus == -1) {
+                    return true;
+                }
+            }
 
-			break;
-		}
-	}
+            break;
+        }
+    }
 
-	return false;
+    return false;
 }
 
 #endif // CONFIG_I2C
diff --git a/pkgs/device/px4/i2c.h b/pkgs/device/px4/i2c.h
index c7c02d1428..1e59da0b9c 100644
--- a/pkgs/device/px4/i2c.h
+++ b/pkgs/device/px4/i2c.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -37,11 +14,11 @@
 
 #if defined(CONFIG_I2C)
 
-#define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES
+#   define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES
 
 struct px4_i2c_bus_t {
-	int bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
-	bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
+    int  bus{-1};     ///< physical bus number (1, ...) (-1 means this is unused)
+    bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
 };
 
 __EXPORT extern const px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS]; ///< board-specific I2C bus configuration
@@ -62,35 +39,41 @@ __EXPORT bool px4_i2c_device_external(const uint32_t device_id);
  * @class I2CBusIterator
  * Iterate over configured I2C buses by the board
  */
-class I2CBusIterator
-{
+class I2CBusIterator {
 public:
-	enum class FilterType {
-		All, ///< specific or all buses
-		InternalBus, ///< specific or all internal buses
-		ExternalBus, ///< specific or all external buses
-	};
+    enum class FilterType {
+        All,         ///< specific or all buses
+        InternalBus, ///< specific or all internal buses
+        ExternalBus, ///< specific or all external buses
+    };
 
-	/**
+    /**
 	 * @param bus specify bus: starts with 1, -1=all. Internal: arch-specific bus numbering is used,
 	 *             external: n-th external bus
 	 */
-	I2CBusIterator(FilterType filter, int bus = -1)
-		: _filter(filter), _bus(bus) {}
+    I2CBusIterator(FilterType filter, int bus = -1) :
+        _filter(filter), _bus(bus) {
+    }
 
-	bool next();
+    bool next();
 
-	const px4_i2c_bus_t &bus() const { return px4_i2c_buses[_index]; }
+    const px4_i2c_bus_t &bus() const {
+        return px4_i2c_buses[_index];
+    }
 
-	int externalBusIndex() const { return _external_bus_counter; }
+    int externalBusIndex() const {
+        return _external_bus_counter;
+    }
 
-	bool external() const { return px4_i2c_bus_external(_bus); }
+    bool external() const {
+        return px4_i2c_bus_external(_bus);
+    }
 
 private:
-	const FilterType _filter;
-	const int _bus;
-	int _index{-1};
-	int _external_bus_counter{0};
+    const FilterType _filter;
+    const int        _bus;
+    int              _index{-1};
+    int              _external_bus_counter{0};
 };
 
 #endif // CONFIG_I2C
diff --git a/pkgs/device/px4/i2c_spi_buses.cpp b/pkgs/device/px4/i2c_spi_buses.cpp
index bc910c11de..74f6e17438 100644
--- a/pkgs/device/px4/i2c_spi_buses.cpp
+++ b/pkgs/device/px4/i2c_spi_buses.cpp
@@ -1,40 +1,17 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2020-2021 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <board_config.h>
 
 #ifndef MODULE_NAME
-#define MODULE_NAME "SPI_I2C"
+#   define MODULE_NAME "SPI_I2C"
 #endif
 
 #include <lib/drivers/device/Device.hpp>
@@ -46,785 +23,766 @@
 #include <pthread.h>
 
 static List<I2CSPIInstance *> i2c_spi_module_instances; ///< list of currently running instances
-static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER;
-
+static pthread_mutex_t        i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER;
 
 I2CSPIDriverConfig::I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-				       const px4::wq_config_t &wq_config_)
-	: module_name(iterator.moduleName()),
-	  devid_driver_index(iterator.devidDriverIndex()),
-	  bus_option(iterator.configuredBusOption()),
-	  bus_type(iterator.busType()),
-	  bus(iterator.bus()),
+                                       const px4::wq_config_t &wq_config_) :
+    module_name(iterator.moduleName()),
+    devid_driver_index(iterator.devidDriverIndex()),
+    bus_option(iterator.configuredBusOption()),
+    bus_type(iterator.busType()),
+    bus(iterator.bus()),
 #if defined(CONFIG_I2C)
-	  i2c_address(cli.i2c_address),
+    i2c_address(cli.i2c_address),
 #endif // CONFIG_I2C
-	  bus_frequency(cli.bus_frequency),
+    bus_frequency(cli.bus_frequency),
 #if defined(CONFIG_SPI)
-	  drdy_gpio(iterator.DRDYGPIO()),
-	  spi_mode(cli.spi_mode),
-	  spi_devid(iterator.devid()),
+    drdy_gpio(iterator.DRDYGPIO()),
+    spi_mode(cli.spi_mode),
+    spi_devid(iterator.devid()),
 #endif // CONFIG_SPI
-	  bus_device_index(iterator.busDeviceIndex()),
-	  rotation(cli.rotation),
-	  quiet_start(cli.quiet_start),
-	  keep_running(cli.keep_running),
-	  custom1(cli.custom1),
-	  custom2(cli.custom2),
-	  custom_data(cli.custom_data),
-	  wq_config(wq_config_)
-{
-
+    bus_device_index(iterator.busDeviceIndex()),
+    rotation(cli.rotation),
+    quiet_start(cli.quiet_start),
+    keep_running(cli.keep_running),
+    custom1(cli.custom1),
+    custom2(cli.custom2),
+    custom_data(cli.custom_data),
+    wq_config(wq_config_) {
 }
 
-const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[])
-{
-	if (getOpt(argc, argv, "") == EOF) {
-		return optArg();
-	}
+const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[]) {
+    if (getOpt(argc, argv, "") == EOF) {
+        return optArg();
+    }
 
-	// unexpected arguments
-	return nullptr;
+    // unexpected arguments
+    return nullptr;
 }
 
-int BusCLIArguments::getOpt(int argc, char *argv[], const char *options)
-{
-	if (_options[0] == 0) { // need to initialize
-		if (!validateConfiguration()) {
-			return EOF;
-		}
+int BusCLIArguments::getOpt(int argc, char *argv[], const char *options) {
+    if (_options[0] == 0) { // need to initialize
+        if (!validateConfiguration()) {
+            return EOF;
+        }
 
-		char *p = (char *)&_options;
+        char *p = (char *)&_options;
 
 #if defined(CONFIG_I2C)
 
-		if (_i2c_support) {
-			*(p++) = 'X'; // external
-			*(p++) = 'I'; // internal
+        if (_i2c_support) {
+            *(p++) = 'X'; // external
+            *(p++) = 'I'; // internal
 
-			if (i2c_address != 0) {
-				*(p++) = 'a'; *(p++) = ':'; // I2C address
-			}
-		}
+            if (i2c_address != 0) {
+                *(p++) = 'a';
+                *(p++) = ':'; // I2C address
+            }
+        }
 
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
 
-		if (_spi_support) {
-			*(p++) = 'S'; // external
-			*(p++) = 's'; // internal
-			*(p++) = 'c'; *(p++) = ':'; // chip-select
-			*(p++) = 'm'; *(p++) = ':'; // spi mode
-		}
+        if (_spi_support) {
+            *(p++) = 'S'; // external
+            *(p++) = 's'; // internal
+            *(p++) = 'c';
+            *(p++) = ':'; // chip-select
+            *(p++) = 'm';
+            *(p++) = ':'; // spi mode
+        }
 
 #endif // CONFIG_SPI
 
-		if (support_keep_running) {
-			*(p++) = 'k';
-		}
-
-		*(p++) = 'b'; *(p++) = ':'; // bus
-		*(p++) = 'f'; *(p++) = ':'; // frequency
-		*(p++) = 'q'; // quiet flag
-
-		// copy all options
-		const char *option = options;
-
-		while (p != _options + sizeof(_options) && *option) {
-			if (*option != ':') {
-				// check for duplicates
-				for (const char *c = _options; c != p; ++c) {
-					if (*c == *option) {
-						PX4_ERR("conflicting option: %c", *c);
-						_options[0] = 0;
-						return EOF;
-					}
-				}
-			}
-
-			*(p++) = *(option++);
-		}
-
-		if (p == _options + sizeof(_options)) {
-			PX4_ERR("too many options");
-			_options[0] = 0;
-			return EOF;
-		}
-
-		*p = '\0';
-	}
-
-	int ch;
-
-	while ((ch = px4_getopt(argc, argv, _options, &_optind, &_optarg)) != EOF) {
-		switch (ch) {
+        if (support_keep_running) {
+            *(p++) = 'k';
+        }
+
+        *(p++) = 'b';
+        *(p++) = ':'; // bus
+        *(p++) = 'f';
+        *(p++) = ':'; // frequency
+        *(p++) = 'q'; // quiet flag
+
+        // copy all options
+        const char *option = options;
+
+        while (p != _options + sizeof(_options) && *option) {
+            if (*option != ':') {
+                // check for duplicates
+                for (const char *c = _options; c != p; ++c) {
+                    if (*c == *option) {
+                        PX4_ERR("conflicting option: %c", *c);
+                        _options[0] = 0;
+                        return EOF;
+                    }
+                }
+            }
+
+            *(p++) = *(option++);
+        }
+
+        if (p == _options + sizeof(_options)) {
+            PX4_ERR("too many options");
+            _options[0] = 0;
+            return EOF;
+        }
+
+        *p = '\0';
+    }
+
+    int ch;
+
+    while ((ch = px4_getopt(argc, argv, _options, &_optind, &_optarg)) != EOF) {
+        switch (ch) {
 #if defined(CONFIG_I2C)
 
-		case 'X':
-			bus_option = I2CSPIBusOption::I2CExternal;
-			break;
+        case 'X':
+            bus_option = I2CSPIBusOption::I2CExternal;
+            break;
 
-		case 'I':
-			bus_option = I2CSPIBusOption::I2CInternal;
-			break;
+        case 'I':
+            bus_option = I2CSPIBusOption::I2CInternal;
+            break;
 
-		case 'a':
-			if (i2c_address == 0) {
-				return ch;
-			}
+        case 'a':
+            if (i2c_address == 0) {
+                return ch;
+            }
 
-			i2c_address = (int)strtol(_optarg, nullptr, 0);
-			break;
+            i2c_address = (int)strtol(_optarg, nullptr, 0);
+            break;
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
 
-		case 'S':
-			bus_option = I2CSPIBusOption::SPIExternal;
-			break;
+        case 'S':
+            bus_option = I2CSPIBusOption::SPIExternal;
+            break;
 
-		case 's':
-			bus_option = I2CSPIBusOption::SPIInternal;
-			break;
+        case 's':
+            bus_option = I2CSPIBusOption::SPIInternal;
+            break;
 
-		case 'c':
-			chipselect = atoi(_optarg);
-			break;
+        case 'c':
+            chipselect = atoi(_optarg);
+            break;
 #endif // CONFIG_SPI
 
-		case 'b':
-			requested_bus = atoi(_optarg);
-			break;
+        case 'b':
+            requested_bus = atoi(_optarg);
+            break;
 
-		case 'f':
-			bus_frequency = 1000 * atoi(_optarg);
-			break;
+        case 'f':
+            bus_frequency = 1000 * atoi(_optarg);
+            break;
 #if defined(CONFIG_SPI)
 
-		case 'm':
-			spi_mode = (spi_mode_e)atoi(_optarg);
-			break;
+        case 'm':
+            spi_mode = (spi_mode_e)atoi(_optarg);
+            break;
 #endif // CONFIG_SPI
 
-		case 'q':
-			quiet_start = true;
-			break;
+        case 'q':
+            quiet_start = true;
+            break;
 
-		case 'k':
-			if (!support_keep_running) {
-				return ch;
-			}
+        case 'k':
+            if (!support_keep_running) {
+                return ch;
+            }
 
-			keep_running = true;
-			break;
+            keep_running = true;
+            break;
 
-		default:
-			if (ch == '?') {
-				// abort further parsing on unknown arguments
-				_optarg = nullptr;
-				return EOF;
-			}
+        default:
+            if (ch == '?') {
+                // abort further parsing on unknown arguments
+                _optarg = nullptr;
+                return EOF;
+            }
 
-			return ch;
-		}
-	}
+            return ch;
+        }
+    }
 
-	if (ch == EOF) {
-		_optarg = argv[_optind];
+    if (ch == EOF) {
+        _optarg = argv[_optind];
 
-		// apply defaults if not provided
-		if (bus_frequency == 0) {
+        // apply defaults if not provided
+        if (bus_frequency == 0) {
 #if defined(CONFIG_I2C)
 
-			if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) {
-				bus_frequency = default_i2c_frequency;
-
-			}
+            if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) {
+                bus_frequency = default_i2c_frequency;
+            }
 
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
 
-			if (bus_option == I2CSPIBusOption::SPIExternal || bus_option == I2CSPIBusOption::SPIInternal) {
-				bus_frequency = default_spi_frequency;
-			}
+            if (bus_option == I2CSPIBusOption::SPIExternal || bus_option == I2CSPIBusOption::SPIInternal) {
+                bus_frequency = default_spi_frequency;
+            }
 
 #endif // CONFIG_SPI
-		}
-	}
+        }
+    }
 
-	return ch;
+    return ch;
 }
 
-bool BusCLIArguments::validateConfiguration()
-{
-	bool success = true;
+bool BusCLIArguments::validateConfiguration() {
+    bool success = true;
 
 #if defined(CONFIG_I2C)
 
-	if (_i2c_support && default_i2c_frequency == -1) {
-		PX4_ERR("Bug: driver %s does not set default_i2c_frequency", px4_get_taskname());
-		success = false;
-	}
+    if (_i2c_support && default_i2c_frequency == -1) {
+        PX4_ERR("Bug: driver %s does not set default_i2c_frequency", px4_get_taskname());
+        success = false;
+    }
 
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
 
-	if (_spi_support && default_spi_frequency == -1) {
-		PX4_ERR("Bug: driver %s does not set default_spi_frequency", px4_get_taskname());
-		success = false;
-	}
+    if (_spi_support && default_spi_frequency == -1) {
+        PX4_ERR("Bug: driver %s does not set default_spi_frequency", px4_get_taskname());
+        success = false;
+    }
 
 #endif // CONFIG_SPI
-	return success;
+    return success;
 }
 
-
-BusInstanceIterator::BusInstanceIterator(const char *module_name,
-		const BusCLIArguments &cli_arguments, uint16_t devid_driver_index)
-	: _module_name(module_name), _bus_option(cli_arguments.bus_option), _devid_driver_index(devid_driver_index),
+BusInstanceIterator::BusInstanceIterator(const char            *module_name,
+                                         const BusCLIArguments &cli_arguments, uint16_t devid_driver_index) :
+    _module_name(module_name), _bus_option(cli_arguments.bus_option), _devid_driver_index(devid_driver_index),
 #if defined(CONFIG_I2C)
-	  _i2c_address(cli_arguments.i2c_address),
+    _i2c_address(cli_arguments.i2c_address),
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
-	  _spi_bus_iterator(spiFilter(cli_arguments.bus_option),
-			    devid_driver_index, cli_arguments.chipselect,
-			    cli_arguments.requested_bus),
+    _spi_bus_iterator(spiFilter(cli_arguments.bus_option),
+                      devid_driver_index, cli_arguments.chipselect,
+                      cli_arguments.requested_bus),
 #endif // CONFIG_SPI
 #if defined(CONFIG_I2C)
-	  _i2c_bus_iterator(i2cFilter(cli_arguments.bus_option), cli_arguments.requested_bus),
+    _i2c_bus_iterator(i2cFilter(cli_arguments.bus_option), cli_arguments.requested_bus),
 #endif // CONFIG_I2C
-	  _current_instance(i2c_spi_module_instances.end())
-{
-	// We lock the module instance list as long as this object is alive, since we iterate over the list.
-	// Locking could be a bit more fine-grained, but the iterator is mostly only used sequentially, so not an issue.
-	pthread_mutex_lock(&i2c_spi_module_instances_mutex);
-	_current_instance = i2c_spi_module_instances.end();
+    _current_instance(i2c_spi_module_instances.end()) {
+    // We lock the module instance list as long as this object is alive, since we iterate over the list.
+    // Locking could be a bit more fine-grained, but the iterator is mostly only used sequentially, so not an issue.
+    pthread_mutex_lock(&i2c_spi_module_instances_mutex);
+    _current_instance = i2c_spi_module_instances.end();
 }
 
-BusInstanceIterator::~BusInstanceIterator()
-{
-	pthread_mutex_unlock(&i2c_spi_module_instances_mutex);
+BusInstanceIterator::~BusInstanceIterator() {
+    pthread_mutex_unlock(&i2c_spi_module_instances_mutex);
 }
 
-bool BusInstanceIterator::next()
-{
-	int bus = -1;
+bool BusInstanceIterator::next() {
+    int bus = -1;
 
-	if (busType() == BOARD_INVALID_BUS) {
-		if (_current_instance == i2c_spi_module_instances.end()) { // either not initialized, or the first instance was removed
-			_current_instance = i2c_spi_module_instances.begin();
+    if (busType() == BOARD_INVALID_BUS) {
+        if (_current_instance == i2c_spi_module_instances.end()) { // either not initialized, or the first instance was removed
+            _current_instance = i2c_spi_module_instances.begin();
 
-		} else {
-			++_current_instance;
-		}
+        } else {
+            ++_current_instance;
+        }
 
-		while (_current_instance != i2c_spi_module_instances.end()) {
-			if (strcmp((*_current_instance)->_module_name, _module_name) == 0 &&
-			    _devid_driver_index == (*_current_instance)->_devid_driver_index) {
-				return true;
-			}
+        while (_current_instance != i2c_spi_module_instances.end()) {
+            if (strcmp((*_current_instance)->_module_name, _module_name) == 0 && _devid_driver_index == (*_current_instance)->_devid_driver_index) {
+                return true;
+            }
 
-			++_current_instance;
-		}
+            ++_current_instance;
+        }
 
-		return false;
+        return false;
 
 #if defined(CONFIG_SPI)
 
-	} else if (busType() == BOARD_SPI_BUS) {
-		if (_spi_bus_iterator.next()) {
-			bus = _spi_bus_iterator.bus().bus;
-		}
+    } else if (busType() == BOARD_SPI_BUS) {
+        if (_spi_bus_iterator.next()) {
+            bus = _spi_bus_iterator.bus().bus;
+        }
 
 #endif // CONFIG_SPI
 #if defined(CONFIG_I2C)
 
-	} else if (busType() == BOARD_I2C_BUS) {
-		if (_i2c_bus_iterator.next()) {
-			bus = _i2c_bus_iterator.bus().bus;
-		}
+    } else if (busType() == BOARD_I2C_BUS) {
+        if (_i2c_bus_iterator.next()) {
+            bus = _i2c_bus_iterator.bus().bus;
+        }
 
 #endif // CONFIG_I2C
-	}
+    }
 
-	if (bus != -1) {
-		// find matching runtime instance
+    if (bus != -1) {
+        // find matching runtime instance
 #if defined(CONFIG_I2C)
-		bool is_i2c = busType() == BOARD_I2C_BUS;
+        bool is_i2c = busType() == BOARD_I2C_BUS;
 #else
-		bool is_i2c = false;
+        bool is_i2c = false;
 #endif
 
-		for (_current_instance = i2c_spi_module_instances.begin(); _current_instance != i2c_spi_module_instances.end();
-		     ++_current_instance) {
-			if (strcmp((*_current_instance)->_module_name, _module_name) != 0) {
-				continue;
-			}
+        for (_current_instance = i2c_spi_module_instances.begin(); _current_instance != i2c_spi_module_instances.end();
+             ++_current_instance) {
+            if (strcmp((*_current_instance)->_module_name, _module_name) != 0) {
+                continue;
+            }
 
-			if (_bus_option == (*_current_instance)->_bus_option && bus == (*_current_instance)->_bus &&
-			    _devid_driver_index == (*_current_instance)->_devid_driver_index &&
-			    busDeviceIndex() == (*_current_instance)->_bus_device_index &&
-			    (!is_i2c
+            if (_bus_option == (*_current_instance)->_bus_option && bus == (*_current_instance)->_bus && _devid_driver_index == (*_current_instance)->_devid_driver_index && busDeviceIndex() == (*_current_instance)->_bus_device_index && (!is_i2c
 #if defined(CONFIG_I2C)
-			     || _i2c_address == (*_current_instance)->_i2c_address
+                                                                                                                                                                                                                                             || _i2c_address == (*_current_instance)->_i2c_address
 #endif // CONFIG_I2C
-			    )
-			   ) {
-				break;
-			}
-		}
+                                                                                                                                                                                                                                             )) {
+                break;
+            }
+        }
 
-		return true;
-	}
+        return true;
+    }
 
-	return false;
+    return false;
 }
 
-int BusInstanceIterator::runningInstancesCount() const
-{
-	int num_instances = 0;
+int BusInstanceIterator::runningInstancesCount() const {
+    int num_instances = 0;
 
-	for (const auto &modules : i2c_spi_module_instances) {
-		if (strcmp(modules->_module_name, _module_name) == 0) {
-			++num_instances;
-		}
-	}
+    for (const auto &modules : i2c_spi_module_instances) {
+        if (strcmp(modules->_module_name, _module_name) == 0) {
+            ++num_instances;
+        }
+    }
 
-	return num_instances;
+    return num_instances;
 }
 
-I2CSPIInstance *BusInstanceIterator::instance() const
-{
-	if (_current_instance == i2c_spi_module_instances.end()) {
-		return nullptr;
-	}
+I2CSPIInstance *BusInstanceIterator::instance() const {
+    if (_current_instance == i2c_spi_module_instances.end()) {
+        return nullptr;
+    }
 
-	return *_current_instance;
+    return *_current_instance;
 }
 
-void BusInstanceIterator::removeInstance()
-{
-	// find previous node
-	List<I2CSPIInstance *>::Iterator previous = i2c_spi_module_instances.begin();
+void BusInstanceIterator::removeInstance() {
+    // find previous node
+    List<I2CSPIInstance *>::Iterator previous = i2c_spi_module_instances.begin();
 
-	while (previous != i2c_spi_module_instances.end() && (*previous)->getSibling() != *_current_instance) {
-		++previous;
-	}
+    while (previous != i2c_spi_module_instances.end() && (*previous)->getSibling() != *_current_instance) {
+        ++previous;
+    }
 
-	i2c_spi_module_instances.remove(*_current_instance);
-	_current_instance = previous; // previous can be i2c_spi_module_instances.end(), which means we removed the first item
+    i2c_spi_module_instances.remove(*_current_instance);
+    _current_instance = previous; // previous can be i2c_spi_module_instances.end(), which means we removed the first item
 }
 
-void BusInstanceIterator::addInstance(I2CSPIInstance *instance)
-{
-	i2c_spi_module_instances.add(instance);
+void BusInstanceIterator::addInstance(I2CSPIInstance *instance) {
+    i2c_spi_module_instances.add(instance);
 }
 
-board_bus_types BusInstanceIterator::busType() const
-{
-	switch (_bus_option) {
-	case I2CSPIBusOption::All:
-		return BOARD_INVALID_BUS;
+board_bus_types BusInstanceIterator::busType() const {
+    switch (_bus_option) {
+    case I2CSPIBusOption::All:
+        return BOARD_INVALID_BUS;
 
 #if defined(CONFIG_I2C)
 
-	case I2CSPIBusOption::I2CInternal:
-	case I2CSPIBusOption::I2CExternal:
-		return BOARD_I2C_BUS;
+    case I2CSPIBusOption::I2CInternal:
+    case I2CSPIBusOption::I2CExternal:
+        return BOARD_I2C_BUS;
 #endif // CONFIG_I2C
 
 #if defined(CONFIG_SPI)
 
-	case I2CSPIBusOption::SPIInternal:
-	case I2CSPIBusOption::SPIExternal:
-		return BOARD_SPI_BUS;
+    case I2CSPIBusOption::SPIInternal:
+    case I2CSPIBusOption::SPIExternal:
+        return BOARD_SPI_BUS;
 #endif // CONFIG_SPI
-	}
+    }
 
-	return BOARD_INVALID_BUS;
+    return BOARD_INVALID_BUS;
 }
 
-int BusInstanceIterator::bus() const
-{
+int BusInstanceIterator::bus() const {
 #if defined(CONFIG_SPI)
 
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.bus().bus;
-	}
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.bus().bus;
+    }
 
 #endif // CONFIG_SPI
 #if defined(CONFIG_I2C)
 
-	if (busType() == BOARD_I2C_BUS) {
-		return _i2c_bus_iterator.bus().bus;
-	}
+    if (busType() == BOARD_I2C_BUS) {
+        return _i2c_bus_iterator.bus().bus;
+    }
 
 #endif // CONFIG_I2C
 
-	return -1;
+    return -1;
 }
 
-uint32_t BusInstanceIterator::devid() const
-{
+uint32_t BusInstanceIterator::devid() const {
 #if defined(CONFIG_SPI)
 
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.devid();
-	}
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.devid();
+    }
 
 #endif // CONFIG_SPI
 
-	return 0;
+    return 0;
 }
 
 #if defined(CONFIG_SPI)
-spi_drdy_gpio_t BusInstanceIterator::DRDYGPIO() const
-{
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.DRDYGPIO();
-	}
+spi_drdy_gpio_t BusInstanceIterator::DRDYGPIO() const {
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.DRDYGPIO();
+    }
 
-	return 0;
+    return 0;
 }
 #endif // CONFIG_SPI
 
-bool BusInstanceIterator::external() const
-{
+bool BusInstanceIterator::external() const {
 #if defined(CONFIG_SPI)
 
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.external();
-	}
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.external();
+    }
 
 #endif // CONFIG_SPI
 
 #if defined(CONFIG_I2C)
 
-	if (busType() == BOARD_I2C_BUS) {
-		return _i2c_bus_iterator.external();
-	}
+    if (busType() == BOARD_I2C_BUS) {
+        return _i2c_bus_iterator.external();
+    }
 
 #endif // CONFIG_I2C
 
-	return false;
+    return false;
 }
 
-int BusInstanceIterator::externalBusIndex() const
-{
+int BusInstanceIterator::externalBusIndex() const {
 #if defined(CONFIG_SPI)
 
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.externalBusIndex();
-	}
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.externalBusIndex();
+    }
 
 #endif // CONFIG_SPI
 
 #if defined(CONFIG_I2C)
 
-	if (busType() == BOARD_I2C_BUS) {
-		return _i2c_bus_iterator.externalBusIndex();
-	}
+    if (busType() == BOARD_I2C_BUS) {
+        return _i2c_bus_iterator.externalBusIndex();
+    }
 
 #endif // CONFIG_I2C
 
-	return 0;
+    return 0;
 }
 
-int BusInstanceIterator::busDeviceIndex() const
-{
+int BusInstanceIterator::busDeviceIndex() const {
 #if defined(CONFIG_SPI)
 
-	if (busType() == BOARD_SPI_BUS) {
-		return _spi_bus_iterator.busDeviceIndex();
-	}
+    if (busType() == BOARD_SPI_BUS) {
+        return _spi_bus_iterator.busDeviceIndex();
+    }
 
 #endif // CONFIG_SPI
 
-	return -1;
+    return -1;
 }
 
 #if defined(CONFIG_I2C)
-I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_option)
-{
-	switch (bus_option) {
-	case I2CSPIBusOption::All: return I2CBusIterator::FilterType::All;
+I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_option) {
+    switch (bus_option) {
+    case I2CSPIBusOption::All:
+        return I2CBusIterator::FilterType::All;
 
-	case I2CSPIBusOption::I2CExternal: return I2CBusIterator::FilterType::ExternalBus;
+    case I2CSPIBusOption::I2CExternal:
+        return I2CBusIterator::FilterType::ExternalBus;
 
-	case I2CSPIBusOption::I2CInternal: return I2CBusIterator::FilterType::InternalBus;
+    case I2CSPIBusOption::I2CInternal:
+        return I2CBusIterator::FilterType::InternalBus;
 
-	default: break;
-	}
+    default:
+        break;
+    }
 
-	return I2CBusIterator::FilterType::All;
+    return I2CBusIterator::FilterType::All;
 }
 #endif // CONFIG_I2C
 
 #if defined(CONFIG_SPI)
-SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_option)
-{
-	switch (bus_option) {
-	case I2CSPIBusOption::SPIExternal: return SPIBusIterator::FilterType::ExternalBus;
+SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_option) {
+    switch (bus_option) {
+    case I2CSPIBusOption::SPIExternal:
+        return SPIBusIterator::FilterType::ExternalBus;
 
-	case I2CSPIBusOption::SPIInternal: return SPIBusIterator::FilterType::InternalBus;
+    case I2CSPIBusOption::SPIInternal:
+        return SPIBusIterator::FilterType::InternalBus;
 
-	default: break;
-	}
+    default:
+        break;
+    }
 
-	return SPIBusIterator::FilterType::InternalBus;
+    return SPIBusIterator::FilterType::InternalBus;
 }
 #endif // CONFIG_SPI
 
 struct I2CSPIDriverInitializing {
-	const I2CSPIDriverConfig &config;
-	I2CSPIDriverBase::instantiate_method instantiate;
-	int runtime_instance;
-	I2CSPIDriverBase *instance{nullptr};
+    const I2CSPIDriverConfig            &config;
+    I2CSPIDriverBase::instantiate_method instantiate;
+    int                                  runtime_instance;
+    I2CSPIDriverBase                    *instance{nullptr};
 };
 
-static void initializer_trampoline(void *argument)
-{
-	I2CSPIDriverInitializing *data = (I2CSPIDriverInitializing *)argument;
-	data->instance = data->instantiate(data->config, data->runtime_instance);
+static void initializer_trampoline(void *argument) {
+    I2CSPIDriverInitializing *data = (I2CSPIDriverInitializing *)argument;
+    data->instance                 = data->instantiate(data->config, data->runtime_instance);
 }
 
 int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator,
-				   void(*print_usage)(), instantiate_method instantiate)
-{
-	if (iterator.configuredBusOption() == I2CSPIBusOption::All) {
-		PX4_ERR("need to specify a bus type");
-		print_usage();
-		return -1;
-	}
+                                   void (*print_usage)(), instantiate_method        instantiate) {
+    if (iterator.configuredBusOption() == I2CSPIBusOption::All) {
+        PX4_ERR("need to specify a bus type");
+        print_usage();
+        return -1;
+    }
 
-	bool started = false;
+    bool started = false;
 
-	while (iterator.next()) {
-		if (iterator.instance()) {
-			PX4_WARN("Already running on bus %i", iterator.bus());
-			continue;
-		}
+    while (iterator.next()) {
+        if (iterator.instance()) {
+            PX4_WARN("Already running on bus %i", iterator.bus());
+            continue;
+        }
 
 
-		device::Device::DeviceId device_id{};
-		device_id.devid_s.bus = iterator.bus();
+        device::Device::DeviceId device_id{};
+        device_id.devid_s.bus = iterator.bus();
 
-		switch (iterator.busType()) {
+        switch (iterator.busType()) {
 #if defined(CONFIG_I2C)
 
-		case BOARD_I2C_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_I2C; break;
+        case BOARD_I2C_BUS:
+            device_id.devid_s.bus_type = device::Device::DeviceBusType_I2C;
+            break;
 #endif // CONFIG_I2C
 
 #if defined(CONFIG_SPI)
 
-		case BOARD_SPI_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_SPI; break;
+        case BOARD_SPI_BUS:
+            device_id.devid_s.bus_type = device::Device::DeviceBusType_SPI;
+            break;
 #endif // CONFIG_SPI
 
-		case BOARD_INVALID_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; break;
-		}
+        case BOARD_INVALID_BUS:
+            device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN;
+            break;
+        }
 
 
-		const px4::wq_config_t &wq_config = px4::device_bus_to_wq(device_id.devid);
-		I2CSPIDriverConfig driver_config{cli, iterator, wq_config};
-		const int runtime_instance = iterator.runningInstancesCount();
-		I2CSPIDriverInitializing initializer_data{driver_config, instantiate, runtime_instance};
-		// initialize the object and bus on the work queue thread - this will also probe for the device
-		px4::WorkItemSingleShot initializer(wq_config, initializer_trampoline, &initializer_data);
-		initializer.ScheduleNow();
-		initializer.wait();
-		I2CSPIDriverBase *instance = initializer_data.instance;
+        const px4::wq_config_t  &wq_config = px4::device_bus_to_wq(device_id.devid);
+        I2CSPIDriverConfig       driver_config{cli, iterator, wq_config};
+        const int                runtime_instance = iterator.runningInstancesCount();
+        I2CSPIDriverInitializing initializer_data{driver_config, instantiate, runtime_instance};
+        // initialize the object and bus on the work queue thread - this will also probe for the device
+        px4::WorkItemSingleShot initializer(wq_config, initializer_trampoline, &initializer_data);
+        initializer.ScheduleNow();
+        initializer.wait();
+        I2CSPIDriverBase *instance = initializer_data.instance;
 
-		if (!instance) {
-			PX4_DEBUG("instantiate failed (no device on bus %i (devid 0x%x)?)", iterator.bus(), iterator.devid());
-			continue;
-		}
+        if (!instance) {
+            PX4_DEBUG("instantiate failed (no device on bus %i (devid 0x%x)?)", iterator.bus(), iterator.devid());
+            continue;
+        }
 
 #if defined(CONFIG_I2C)
 
-		if (cli.i2c_address != 0 && instance->_i2c_address == 0) {
-			PX4_ERR("Bug: driver %s does not pass the I2C address to I2CSPIDriverBase", instance->ItemName());
-		}
+        if (cli.i2c_address != 0 && instance->_i2c_address == 0) {
+            PX4_ERR("Bug: driver %s does not pass the I2C address to I2CSPIDriverBase", instance->ItemName());
+        }
 
 #endif // CONFIG_I2C
 
-		iterator.addInstance(instance);
-		started = true;
+        iterator.addInstance(instance);
+        started = true;
 
-		// print some info that we are running
-		switch (iterator.busType()) {
+        // print some info that we are running
+        switch (iterator.busType()) {
 #if defined(CONFIG_I2C)
 
-		case BOARD_I2C_BUS:
-			PX4_INFO_RAW("%s #%i on I2C bus %d", instance->ItemName(), runtime_instance, iterator.bus());
+        case BOARD_I2C_BUS:
+            PX4_INFO_RAW("%s #%i on I2C bus %d", instance->ItemName(), runtime_instance, iterator.bus());
 
-			if (iterator.external()) {
-				PX4_INFO_RAW(" (external)");
-			}
+            if (iterator.external()) {
+                PX4_INFO_RAW(" (external)");
+            }
 
-			if (cli.i2c_address != 0) {
-				PX4_INFO_RAW(" address 0x%X", cli.i2c_address);
-			}
+            if (cli.i2c_address != 0) {
+                PX4_INFO_RAW(" address 0x%X", cli.i2c_address);
+            }
 
-			if (cli.rotation != 0) {
-				PX4_INFO_RAW(" rotation %d", cli.rotation);
-			}
+            if (cli.rotation != 0) {
+                PX4_INFO_RAW(" rotation %d", cli.rotation);
+            }
 
-			PX4_INFO_RAW("\n");
+            PX4_INFO_RAW("\n");
 
-			break;
+            break;
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
 
-		case BOARD_SPI_BUS:
-			PX4_INFO_RAW("%s #%i on SPI bus %d", instance->ItemName(), runtime_instance, iterator.bus());
+        case BOARD_SPI_BUS:
+            PX4_INFO_RAW("%s #%i on SPI bus %d", instance->ItemName(), runtime_instance, iterator.bus());
 
-			if (iterator.external()) {
-				PX4_INFO_RAW(" (external, equal to '-b %i')", iterator.externalBusIndex());
-			}
+            if (iterator.external()) {
+                PX4_INFO_RAW(" (external, equal to '-b %i')", iterator.externalBusIndex());
+            }
 
-			if (cli.rotation != 0) {
-				PX4_INFO_RAW(" rotation %d", cli.rotation);
-			}
+            if (cli.rotation != 0) {
+                PX4_INFO_RAW(" rotation %d", cli.rotation);
+            }
 
-			PX4_INFO_RAW("\n");
+            PX4_INFO_RAW("\n");
 
-			break;
+            break;
 #endif // CONFIG_SPI
 
-		case BOARD_INVALID_BUS:
-			break;
-		}
-	}
+        case BOARD_INVALID_BUS:
+            break;
+        }
+    }
 
-	if (!started && !cli.quiet_start) {
-		static constexpr char no_instance_started[] {"no instance started (no device on bus?)"};
+    if (!started && !cli.quiet_start) {
+        static constexpr char no_instance_started[]{"no instance started (no device on bus?)"};
 
-		if (iterator.external()) {
-			PX4_WARN("%s: %s", px4_get_taskname(), no_instance_started);
+        if (iterator.external()) {
+            PX4_WARN("%s: %s", px4_get_taskname(), no_instance_started);
 
-		} else {
-			PX4_ERR("%s: %s", px4_get_taskname(), no_instance_started);
-		}
+        } else {
+            PX4_ERR("%s: %s", px4_get_taskname(), no_instance_started);
+        }
 
 #if defined(CONFIG_I2C)
 
-		if (iterator.busType() == BOARD_I2C_BUS && cli.i2c_address == 0) {
-			PX4_ERR("%s: driver does not set i2c address", px4_get_taskname());
-		}
+        if (iterator.busType() == BOARD_I2C_BUS && cli.i2c_address == 0) {
+            PX4_ERR("%s: driver does not set i2c address", px4_get_taskname());
+        }
 
 #endif // CONFIG_I2C
-	}
+    }
 
-	return started ? 0 : -1;
+    return started ? 0 : -1;
 }
 
-int I2CSPIDriverBase::module_stop(BusInstanceIterator &iterator)
-{
-	bool is_running = false;
-
-	while (iterator.next()) {
-		if (iterator.instance()) {
-			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
-			instance->request_stop_and_wait();
-			delete iterator.instance();
-			iterator.removeInstance();
-			is_running = true;
-		}
-	}
-
-	if (!is_running) {
-		PX4_ERR("Not running");
-		return -1;
-	}
-
-	return 0;
+int I2CSPIDriverBase::module_stop(BusInstanceIterator &iterator) {
+    bool is_running = false;
+
+    while (iterator.next()) {
+        if (iterator.instance()) {
+            I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+            instance->request_stop_and_wait();
+            delete iterator.instance();
+            iterator.removeInstance();
+            is_running = true;
+        }
+    }
+
+    if (!is_running) {
+        PX4_ERR("Not running");
+        return -1;
+    }
+
+    return 0;
 }
 
-int I2CSPIDriverBase::module_status(BusInstanceIterator &iterator)
-{
-	bool is_running = false;
+int I2CSPIDriverBase::module_status(BusInstanceIterator &iterator) {
+    bool is_running = false;
 
-	while (iterator.next()) {
-		if (iterator.instance()) {
-			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
-			instance->print_status();
-			is_running = true;
-		}
-	}
+    while (iterator.next()) {
+        if (iterator.instance()) {
+            I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+            instance->print_status();
+            is_running = true;
+        }
+    }
 
-	if (!is_running) {
-		PX4_INFO("Not running");
-		return -1;
-	}
+    if (!is_running) {
+        PX4_INFO("Not running");
+        return -1;
+    }
 
-	return 0;
+    return 0;
 }
 
 struct custom_method_data_t {
-	I2CSPIDriverBase *instance;
-	const BusCLIArguments &cli;
+    I2CSPIDriverBase      *instance;
+    const BusCLIArguments &cli;
 };
 
-void I2CSPIDriverBase::custom_method_trampoline(void *argument)
-{
-	custom_method_data_t *data = (custom_method_data_t *)argument;
-	data->instance->custom_method(data->cli);
+void I2CSPIDriverBase::custom_method_trampoline(void *argument) {
+    custom_method_data_t *data = (custom_method_data_t *)argument;
+    data->instance->custom_method(data->cli);
 }
 
 int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator,
-		bool run_on_work_queue)
-{
-	while (iterator.next()) {
-		if (iterator.instance()) {
-			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
-
-			if (run_on_work_queue) {
-				custom_method_data_t data{instance, cli};
-				px4::WorkItemSingleShot runner(*instance, custom_method_trampoline, &data);
-				runner.ScheduleNow();
-				runner.wait();
-
-			} else {
-				instance->custom_method(cli);
-			}
-		}
-	}
-
-	return 0;
+                                           bool run_on_work_queue) {
+    while (iterator.next()) {
+        if (iterator.instance()) {
+            I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+
+            if (run_on_work_queue) {
+                custom_method_data_t    data{instance, cli};
+                px4::WorkItemSingleShot runner(*instance, custom_method_trampoline, &data);
+                runner.ScheduleNow();
+                runner.wait();
+
+            } else {
+                instance->custom_method(cli);
+            }
+        }
+    }
+
+    return 0;
 }
 
-void I2CSPIDriverBase::print_status()
-{
+void I2CSPIDriverBase::print_status() {
 #if defined(CONFIG_I2C)
 
-	if (_bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal) {
-		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address());
-		return;
-	}
+    if (_bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal) {
+        PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address());
+        return;
+    }
 
 #endif // CONFIG_I2C
 
 #if defined(CONFIG_SPI)
 
-	if (_bus_option == I2CSPIBusOption::SPIExternal || _bus_option == I2CSPIBusOption::SPIInternal) {
-		PX4_INFO("Running on SPI Bus %i", _bus);
-		return;
-	}
+    if (_bus_option == I2CSPIBusOption::SPIExternal || _bus_option == I2CSPIBusOption::SPIInternal) {
+        PX4_INFO("Running on SPI Bus %i", _bus);
+        return;
+    }
 
 #endif // CONFIG_SPI
 }
 
-void I2CSPIDriverBase::request_stop_and_wait()
-{
-	_task_should_exit.store(true);
-	ScheduleNow(); // wake up the task (in case it is not scheduled anymore or just to be faster)
-	unsigned int i = 0;
+void I2CSPIDriverBase::request_stop_and_wait() {
+    _task_should_exit.store(true);
+    ScheduleNow(); // wake up the task (in case it is not scheduled anymore or just to be faster)
+    unsigned int i = 0;
 
-	do {
-		px4_usleep(20000); // 20 ms
-		// wait at most 2 sec
-	} while (++i < 100 && !_task_exited.load());
+    do {
+        px4_usleep(20000); // 20 ms
+                           // wait at most 2 sec
+    } while (++i < 100 && !_task_exited.load());
 
-	if (i >= 100) {
-		PX4_ERR("Module did not respond to stop request");
-	}
+    if (i >= 100) {
+        PX4_ERR("Module did not respond to stop request");
+    }
 }
diff --git a/pkgs/device/px4/i2c_spi_buses.h b/pkgs/device/px4/i2c_spi_buses.h
index aa6dc90703..17492831b0 100644
--- a/pkgs/device/px4/i2c_spi_buses.h
+++ b/pkgs/device/px4/i2c_spi_buses.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -45,26 +22,26 @@
 #include <px4_platform_common/sem.h>
 
 #if defined(CONFIG_I2C)
-# include "i2c.h"
+#   include "i2c.h"
 #endif // CONFIG_I2C
 
 #if defined(CONFIG_SPI)
-# include "spi.h"
+#   include "spi.h"
 #endif // CONFIG_SPI
 
 #if defined(CONFIG_SPI)
-# include <drivers/device/spi.h>
+#   include <drivers/device/spi.h>
 #endif // CONFIG_SPI
 
 enum class I2CSPIBusOption : uint8_t {
-	All = 0, ///< select all runnning instances
+    All = 0, ///< select all runnning instances
 #if defined(CONFIG_I2C)
-	I2CInternal,
-	I2CExternal,
+    I2CInternal,
+    I2CExternal,
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
-	SPIInternal,
-	SPIExternal,
+    SPIInternal,
+    SPIExternal,
 #endif // CONFIG_SPI
 };
 
@@ -72,147 +49,152 @@ class BusCLIArguments;
 class BusInstanceIterator;
 
 struct I2CSPIDriverConfig {
-	I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-			   const px4::wq_config_t &wq_config_);
-
-	const char *module_name;
-	uint16_t devid_driver_index;
-	I2CSPIBusOption bus_option;
-	board_bus_types bus_type;
-	int bus;
+    I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+                       const px4::wq_config_t &wq_config_);
+
+    const char     *module_name;
+    uint16_t        devid_driver_index;
+    I2CSPIBusOption bus_option;
+    board_bus_types bus_type;
+    int             bus;
 #if defined(CONFIG_I2C)
-	uint8_t i2c_address;
+    uint8_t i2c_address;
 #endif // CONFIG_I2C
-	int bus_frequency;
+    int bus_frequency;
 #if defined(CONFIG_SPI)
-	spi_drdy_gpio_t drdy_gpio;
-	spi_mode_e spi_mode;
-	uint32_t spi_devid;
+    spi_drdy_gpio_t drdy_gpio;
+    spi_mode_e      spi_mode;
+    uint32_t        spi_devid;
 #endif // CONFIG_SPI
-	int bus_device_index;
+    int bus_device_index;
 
-	Rotation rotation;
+    Rotation rotation;
 
-	bool quiet_start;
-	bool keep_running;
+    bool quiet_start;
+    bool keep_running;
 
-	int custom1;
-	int custom2;
-	void *custom_data;
+    int   custom1;
+    int   custom2;
+    void *custom_data;
 
-	const px4::wq_config_t &wq_config;
+    const px4::wq_config_t &wq_config;
 };
 
 /**
  * @class I2CSPIInstance
  * I2C/SPI driver instance used by BusInstanceIterator to find running instances.
  */
-class I2CSPIInstance : public ListNode<I2CSPIInstance *>
-{
+class I2CSPIInstance : public ListNode<I2CSPIInstance *> {
 public:
-	virtual ~I2CSPIInstance() = default;
+    virtual ~I2CSPIInstance() = default;
 #if defined(CONFIG_I2C)
-	virtual int8_t  get_i2c_address() {return _i2c_address;}
+    virtual int8_t get_i2c_address() {
+        return _i2c_address;
+    }
 #endif // CONFIG_I2C
+
 private:
-	I2CSPIInstance(const I2CSPIDriverConfig &config)
-		: _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus),
-		  _devid_driver_index(config.devid_driver_index), _bus_device_index(config.bus_device_index)
+    I2CSPIInstance(const I2CSPIDriverConfig &config) :
+        _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus),
+        _devid_driver_index(config.devid_driver_index), _bus_device_index(config.bus_device_index)
 #if defined(CONFIG_I2C)
-		, _i2c_address(config.i2c_address)
+        ,
+        _i2c_address(config.i2c_address)
 #endif // CONFIG_I2C
-	{}
+    {
+    }
 
 
-	friend class BusInstanceIterator;
-	friend class I2CSPIDriverBase;
+    friend class BusInstanceIterator;
+    friend class I2CSPIDriverBase;
 
-	const char *_module_name;
-	const I2CSPIBusOption _bus_option;
-	const int _bus;
-	const uint16_t _devid_driver_index;
-	const int8_t _bus_device_index;
+    const char           *_module_name;
+    const I2CSPIBusOption _bus_option;
+    const int             _bus;
+    const uint16_t        _devid_driver_index;
+    const int8_t          _bus_device_index;
 #if defined(CONFIG_I2C)
-	const int8_t _i2c_address; ///< I2C address (optional)
-#endif // CONFIG_I2C
+    const int8_t _i2c_address; ///< I2C address (optional)
+#endif                         // CONFIG_I2C
 };
 
-class BusCLIArguments
-{
+class BusCLIArguments {
 public:
-	BusCLIArguments(bool i2c_support, bool spi_support)
+    BusCLIArguments(bool i2c_support, bool spi_support)
 #if defined(CONFIG_I2C) || defined(CONFIG_SPI)
-		:
+        :
 #endif // CONFIG_I2C || CONFIG_SPI
 #if defined(CONFIG_I2C)
-		_i2c_support(i2c_support)
+        _i2c_support(i2c_support)
 #endif // CONFIG_I2C
 #if defined(CONFIG_I2C) && defined(CONFIG_SPI)
-		,
+        ,
 #endif // CONFIG_I2C && CONFIG_SPI
 #if defined(CONFIG_SPI)
-		_spi_support(spi_support)
+        _spi_support(spi_support)
 #endif // CONFIG_SPI
-	{}
+    {
+    }
 
-	/**
+    /**
 	 * Parse CLI arguments (for drivers that don't need any custom arguments, otherwise getopt() should be used)
 	 * @return command (e.g. "start") or nullptr on error or unknown argument
 	 */
-	const char *parseDefaultArguments(int argc, char *argv[]);
+    const char *parseDefaultArguments(int argc, char *argv[]);
 
-	/**
+    /**
 	 * Like px4_getopt(), but adds and handles i2c/spi driver-specific arguments
 	 */
-	int getOpt(int argc, char *argv[], const char *options);
+    int getOpt(int argc, char *argv[], const char *options);
 
-	/**
+    /**
 	 * returns the current optional argument (for options like 'T:'), or the command (e.g. "start")
 	 * @return nullptr or argument/command
 	 */
-	const char *optArg() const { return _optarg; }
+    const char *optArg() const {
+        return _optarg;
+    }
 
-
-	I2CSPIBusOption bus_option{I2CSPIBusOption::All};
-	int requested_bus{-1};
-	int bus_frequency{0};
+    I2CSPIBusOption bus_option{I2CSPIBusOption::All};
+    int             requested_bus{-1};
+    int             bus_frequency{0};
 #if defined(CONFIG_SPI)
-	int chipselect {-1};
-	spi_mode_e spi_mode{SPIDEV_MODE3};
-#endif // CONFIG_SPI
+    int        chipselect{-1};
+    spi_mode_e spi_mode{SPIDEV_MODE3};
+#endif                                // CONFIG_SPI
 #if defined(CONFIG_I2C)
-	uint8_t i2c_address {0}; ///< I2C address (a driver must set the default address)
-#endif // CONFIG_I2C
-	bool quiet_start {false}; ///< do not print a message when startup fails
-	bool keep_running{false}; ///< keep driver running even if no device is detected on startup
+    uint8_t i2c_address{0};           ///< I2C address (a driver must set the default address)
+#endif                                // CONFIG_I2C
+    bool quiet_start{false};          ///< do not print a message when startup fails
+    bool keep_running{false};         ///< keep driver running even if no device is detected on startup
 
-	Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*)
+    Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*)
 
-	int custom1{0}; ///< driver-specific custom argument
-	int custom2{0}; ///< driver-specific custom argument
-	void *custom_data{nullptr}; ///< driver-specific custom argument
+    int   custom1{0};                 ///< driver-specific custom argument
+    int   custom2{0};                 ///< driver-specific custom argument
+    void *custom_data{nullptr};       ///< driver-specific custom argument
 
-	// driver defaults, if not specified via CLI
+                                      // driver defaults, if not specified via CLI
 #if defined(CONFIG_SPI)
-	int default_spi_frequency {-1}; ///< default spi bus frequency (driver needs to set this) [Hz]
-#endif // CONFIG_SPI
+    int default_spi_frequency{-1};    ///< default spi bus frequency (driver needs to set this) [Hz]
+#endif                                // CONFIG_SPI
 #if defined(CONFIG_I2C)
-	int default_i2c_frequency {-1}; ///< default i2c bus frequency (driver needs to set this) [Hz]
-#endif // CONFIG_I2C
+    int default_i2c_frequency{-1};    ///< default i2c bus frequency (driver needs to set this) [Hz]
+#endif                                // CONFIG_I2C
 
-	bool support_keep_running{false}; ///< true if keep_running (see above) is supported
+    bool support_keep_running{false}; ///< true if keep_running (see above) is supported
 
 private:
-	bool validateConfiguration();
+    bool validateConfiguration();
 
-	char _options[32] {};
-	int _optind{1};
-	const char *_optarg{nullptr};
+    char        _options[32]{};
+    int         _optind{1};
+    const char *_optarg{nullptr};
 #if defined(CONFIG_I2C)
-	const bool _i2c_support;
+    const bool _i2c_support;
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
-	const bool _spi_support;
+    const bool _spi_support;
 #endif // CONFIG_SPI
 };
 
@@ -220,162 +202,178 @@ class BusCLIArguments
  * @class BusInstanceIterator
  * Iterate over running instances and/or configured I2C/SPI buses with given filter options.
  */
-class BusInstanceIterator
-{
+class BusInstanceIterator {
 public:
-	BusInstanceIterator(const char *module_name, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index);
-	~BusInstanceIterator();
+    BusInstanceIterator(const char *module_name, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index);
+    ~BusInstanceIterator();
 
-	I2CSPIBusOption configuredBusOption() const { return _bus_option; }
+    I2CSPIBusOption configuredBusOption() const {
+        return _bus_option;
+    }
 
-	int runningInstancesCount() const;
+    int runningInstancesCount() const;
 
-	bool next();
+    bool next();
 
-	I2CSPIInstance *instance() const;
-	void removeInstance();
-	board_bus_types busType() const;
-	int bus() const;
-	uint32_t devid() const;
+    I2CSPIInstance *instance() const;
+    void            removeInstance();
+    board_bus_types busType() const;
+    int             bus() const;
+    uint32_t        devid() const;
 
 #if defined(CONFIG_SPI)
-	spi_drdy_gpio_t DRDYGPIO() const;
+    spi_drdy_gpio_t DRDYGPIO() const;
 #endif // CONFIG_SPI
 
-	bool external() const;
-	int externalBusIndex() const;
-	int busDeviceIndex() const;
+    bool external() const;
+    int  externalBusIndex() const;
+    int  busDeviceIndex() const;
 
-	void addInstance(I2CSPIInstance *instance);
+    void addInstance(I2CSPIInstance *instance);
 
 #if defined(CONFIG_I2C)
-	static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option);
+    static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option);
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
-	static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option);
+    static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option);
 #endif // CONFIG_SPI
 
-	const char *moduleName() const { return _module_name; }
-	uint16_t devidDriverIndex() const { return _devid_driver_index; }
+    const char *moduleName() const {
+        return _module_name;
+    }
+
+    uint16_t devidDriverIndex() const {
+        return _devid_driver_index;
+    }
 
 private:
-	const char *_module_name;
-	const I2CSPIBusOption _bus_option;
-	const uint16_t _devid_driver_index;
+    const char           *_module_name;
+    const I2CSPIBusOption _bus_option;
+    const uint16_t        _devid_driver_index;
 #if defined(CONFIG_I2C)
-	const uint8_t _i2c_address;
+    const uint8_t _i2c_address;
 #endif // CONFIG_I2C
 #if defined(CONFIG_SPI)
-	SPIBusIterator _spi_bus_iterator;
+    SPIBusIterator _spi_bus_iterator;
 #endif // CONFIG_SPI
 #if defined(CONFIG_I2C)
-	I2CBusIterator _i2c_bus_iterator;
+    I2CBusIterator _i2c_bus_iterator;
 #endif // CONFIG_I2C
-	List<I2CSPIInstance *>::Iterator _current_instance;
+    List<I2CSPIInstance *>::Iterator _current_instance;
 };
 
 /**
  * @class I2CSPIDriverBase
  * Base class for I2C/SPI driver modules (non-templated, used by I2CSPIDriver)
  */
-class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance
-{
+class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance {
 public:
-	I2CSPIDriverBase(const I2CSPIDriverConfig &config)
-		: ScheduledWorkItem(config.module_name, config.wq_config),
-		  I2CSPIInstance(config) {}
+    I2CSPIDriverBase(const I2CSPIDriverConfig &config) :
+        ScheduledWorkItem(config.module_name, config.wq_config),
+        I2CSPIInstance(config) {
+    }
 
-	static int module_stop(BusInstanceIterator &iterator);
-	static int module_status(BusInstanceIterator &iterator);
-	static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator,
-					bool run_on_work_queue = true);
+    static int module_stop(BusInstanceIterator &iterator);
+    static int module_status(BusInstanceIterator &iterator);
+    static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator,
+                                    bool run_on_work_queue = true);
+
+    using instantiate_method = I2CSPIDriverBase *(*)(const I2CSPIDriverConfig &config, int runtime_instance);
 
-	using instantiate_method = I2CSPIDriverBase * (*)(const I2CSPIDriverConfig &config, int runtime_instance);
 protected:
-	virtual ~I2CSPIDriverBase() = default;
+    virtual ~I2CSPIDriverBase() = default;
 
-	virtual void print_status();
+    virtual void print_status();
 
-	virtual void custom_method(const BusCLIArguments &cli) {}
+    virtual void custom_method(const BusCLIArguments &cli) {
+    }
 
-	/**
+    /**
 	 * Exiting the module. A driver can override this, for example to unregister interrupt callbacks.
 	 * This will be called from the work queue.
 	 * A module overriding this, needs to call I2CSPIDriverBase::exit_and_cleanup() as the very last statement.
 	 */
-	virtual void exit_and_cleanup() { ScheduleClear(); _task_exited.store(true); }
+    virtual void exit_and_cleanup() {
+        ScheduleClear();
+        _task_exited.store(true);
+    }
 
-	bool should_exit() const { return _task_should_exit.load(); }
+    bool should_exit() const {
+        return _task_should_exit.load();
+    }
 
-	static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator, void(*print_usage)(),
-				instantiate_method instantiate);
+    static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator, void (*print_usage)(),
+                            instantiate_method instantiate);
 
 private:
-	static void custom_method_trampoline(void *argument);
+    static void custom_method_trampoline(void *argument);
 
-	void request_stop_and_wait();
+    void request_stop_and_wait();
 
-	px4::atomic_bool _task_should_exit{false};
-	px4::atomic_bool _task_exited{false};
+    px4::atomic_bool _task_should_exit{false};
+    px4::atomic_bool _task_exited{false};
 };
 
 /**
  * @class I2CSPIDriver
  * Base class for I2C/SPI driver modules
  */
-template<class T>
-class I2CSPIDriver : public I2CSPIDriverBase
-{
+template <class T>
+class I2CSPIDriver : public I2CSPIDriverBase {
 public:
-	static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator)
-	{
-		return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, InstantiateHelper<T>::m);
-	}
+    static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator) {
+        return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, InstantiateHelper<T>::m);
+    }
 
 protected:
-	I2CSPIDriver(const I2CSPIDriverConfig &config)
-		: I2CSPIDriverBase(config) {}
+    I2CSPIDriver(const I2CSPIDriverConfig &config) :
+        I2CSPIDriverBase(config) {
+    }
 
-	virtual ~I2CSPIDriver() = default;
+    virtual ~I2CSPIDriver() = default;
 
-	// *INDENT-OFF* remove once there's astyle >3.1 in CI
-	void Run() final
-	{
-		static_cast<T *>(this)->RunImpl();
+    // *INDENT-OFF* remove once there's astyle >3.1 in CI
+    void Run() final {
+        static_cast<T *>(this)->RunImpl();
 
-		if (should_exit()) {
-			exit_and_cleanup();
-		}
-	}
-	// *INDENT-ON*
-private:
+        if (should_exit()) {
+            exit_and_cleanup();
+        }
+    }
 
-	// SFINAE to use R::instantiate if it exists, and R::instantiate_default otherwise
-	template <typename R>
-	class InstantiateHelper
-	{
-		template <typename C>
-		static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; }
-		template <typename C>
-		static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; }
-	public:
-		static constexpr I2CSPIDriverBase::instantiate_method m = get<R>(0);
-	};
-
-	static I2CSPIDriverBase *instantiate_default(const I2CSPIDriverConfig &config, int runtime_instance)
-	{
-		T *instance = new T(config);
-
-		if (!instance) {
-			PX4_ERR("alloc failed");
-			return nullptr;
-		}
-
-		if (OK != instance->init()) {
-			delete instance;
-			return nullptr;
-		}
-
-		return instance;
-	}
+    // *INDENT-ON*
+
+private:
+    // SFINAE to use R::instantiate if it exists, and R::instantiate_default otherwise
+    template <typename R>
+    class InstantiateHelper {
+        template <typename C>
+        static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) {
+            return &C::instantiate;
+        }
+
+        template <typename C>
+        static constexpr I2CSPIDriverBase::instantiate_method get(...) {
+            return &C::instantiate_default;
+        }
+
+    public:
+        static constexpr I2CSPIDriverBase::instantiate_method m = get<R>(0);
+    };
+
+    static I2CSPIDriverBase *instantiate_default(const I2CSPIDriverConfig &config, int runtime_instance) {
+        T *instance = new T(config);
+
+        if (!instance) {
+            PX4_ERR("alloc failed");
+            return nullptr;
+        }
+
+        if (OK != instance->init()) {
+            delete instance;
+            return nullptr;
+        }
+
+        return instance;
+    }
 };
diff --git a/pkgs/device/px4/spi.cpp b/pkgs/device/px4/spi.cpp
index 62713b0798..db8dccd004 100644
--- a/pkgs/device/px4/spi.cpp
+++ b/pkgs/device/px4/spi.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <board_config.h>
 
diff --git a/pkgs/device/px4/spi.h b/pkgs/device/px4/spi.h
index 6664409822..2116732a02 100644
--- a/pkgs/device/px4/spi.h
+++ b/pkgs/device/px4/spi.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020-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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -43,40 +20,40 @@
  * They match with corresponding definitions in NuttX.
  * 'type' is typically PX4_SPI_DEVICE_ID for PX4 drivers, and 'index' is used for the driver (DRV_*) or chip-select index.
  */
-#define PX4_SPIDEV_ID(type, index)  ((((type) & 0xffff) << 16) | ((index) & 0xffff))
-#define PX4_SPI_DEVICE_ID         (1 << 12)
-#define PX4_SPI_DEV_ID(devid)     ((devid) & 0xffff)
-#define PX4_SPIDEVID_TYPE(devid) (((uint32_t)(devid) >> 16) & 0xffff)
+#   define PX4_SPIDEV_ID(type, index) ((((type) & 0xffff) << 16) | ((index) & 0xffff))
+#   define PX4_SPI_DEVICE_ID          (1 << 12)
+#   define PX4_SPI_DEV_ID(devid)      ((devid) & 0xffff)
+#   define PX4_SPIDEVID_TYPE(devid)   (((uint32_t)(devid) >> 16) & 0xffff)
 
 typedef uint32_t spi_drdy_gpio_t;
 
-#define SPI_BUS_MAX_DEVICES 6
+#   define SPI_BUS_MAX_DEVICES 6
+
 struct px4_spi_bus_device_t {
-	uint32_t cs_gpio; ///< chip-select GPIO (0 if this device is not used)
-	spi_drdy_gpio_t drdy_gpio; ///< data ready GPIO (0 if not set)
-	uint32_t devid; ///< SPIDEV_ID(type,index). For PX4 devices on NuttX: index is the device type, and for external buses the CS index
-	uint16_t devtype_driver; ///< driver device type, e.g. DRV_IMU_DEVTYPE_ICM20689 (on NuttX: PX4_SPI_DEV_ID(devid) == devtype_driver)
+    uint32_t        cs_gpio;        ///< chip-select GPIO (0 if this device is not used)
+    spi_drdy_gpio_t drdy_gpio;      ///< data ready GPIO (0 if not set)
+    uint32_t        devid;          ///< SPIDEV_ID(type,index). For PX4 devices on NuttX: index is the device type, and for external buses the CS index
+    uint16_t        devtype_driver; ///< driver device type, e.g. DRV_IMU_DEVTYPE_ICM20689 (on NuttX: PX4_SPI_DEV_ID(devid) == devtype_driver)
 };
 
 struct px4_spi_bus_devices_t {
-	px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
+    px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
 };
 
 struct px4_spi_bus_t {
-	px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
-	uint32_t power_enable_gpio{0}; ///< GPIO (if non-zero) to control the power of the attached devices on this bus (0 means power is off)
-	int8_t bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
-	bool is_external; ///< static external configuration. Use px4_spi_bus_external() to check if a bus is really external
-	bool requires_locking; ///< whether the bus should be locked during transfers (true if NuttX drivers access the bus)
+    px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
+    uint32_t             power_enable_gpio{0}; ///< GPIO (if non-zero) to control the power of the attached devices on this bus (0 means power is off)
+    int8_t               bus{-1};              ///< physical bus number (1, ...) (-1 means this is unused)
+    bool                 is_external;          ///< static external configuration. Use px4_spi_bus_external() to check if a bus is really external
+    bool                 requires_locking;     ///< whether the bus should be locked during transfers (true if NuttX drivers access the bus)
 };
 
-
 struct px4_spi_bus_all_hw_t {
-	px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
-	int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
+    px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
+    int           board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
 };
 
-#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
+#   if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 /**
  * initialze px4_spi_buses from px4_spi_buses_all_hw and the hardware version.
  * Call this on early boot before anything else accesses px4_spi_buses (e.g. from stm32_spiinitialize).
@@ -84,13 +61,15 @@ struct px4_spi_bus_all_hw_t {
 __EXPORT void px4_set_spi_buses_from_hw_version();
 
 __EXPORT extern const px4_spi_bus_all_hw_t
-px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS]; ///< board-specific SPI bus configuration all hw versions
+    px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS]; ///< board-specific SPI bus configuration all hw versions
+
+__EXPORT extern const px4_spi_bus_t *px4_spi_buses;      ///< board-specific SPI bus configuration for current board revision
+#   else
+static inline void px4_set_spi_buses_from_hw_version() {
+}
 
-__EXPORT extern const px4_spi_bus_t *px4_spi_buses; ///< board-specific SPI bus configuration for current board revision
-#else
-static inline void px4_set_spi_buses_from_hw_version() {}
 __EXPORT extern const px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS]; ///< board-specific SPI bus configuration
-#endif
+#   endif
 
 
 /**
@@ -113,31 +92,28 @@ __EXPORT bool px4_spi_bus_external(const px4_spi_bus_t &bus);
 /**
  * runtime-check if a board has a specific bus as external.
  */
-static inline bool px4_spi_bus_external(int bus)
-{
-	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
-		if (px4_spi_buses[i].bus == bus) {
-			return px4_spi_bus_external(px4_spi_buses[i]);
-		}
-	}
-
-	return true;
+static inline bool px4_spi_bus_external(int bus) {
+    for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+        if (px4_spi_buses[i].bus == bus) {
+            return px4_spi_bus_external(px4_spi_buses[i]);
+        }
+    }
+
+    return true;
 }
 
-
 /**
  * @class SPIBusIterator
  * Iterate over configured SPI buses by the board
  */
-class SPIBusIterator
-{
+class SPIBusIterator {
 public:
-	enum class FilterType {
-		InternalBus, ///< specific or all internal buses
-		ExternalBus, ///< specific external bus + CS index
-	};
+    enum class FilterType {
+        InternalBus, ///< specific or all internal buses
+        ExternalBus, ///< specific external bus + CS index
+    };
 
-	/**
+    /**
 	 * Constructor
 	 * Note: only for devices of type PX4_SPI_DEVICE_ID
 	 * @param filter
@@ -146,32 +122,46 @@ class SPIBusIterator
 	 * @param bus starts with 1 (-1=all, but only for internal). Numbering for internal is arch-specific, for external
 	 *            it is the n-th external bus.
 	 */
-	SPIBusIterator(FilterType filter, uint16_t devid_driver_index, int16_t chipselect = -1, int bus = -1)
-		: _filter(filter), _devid_driver_index(devid_driver_index),
-		  _chipselect(chipselect),
-		  _bus(filter == FilterType::ExternalBus && bus == -1 ? 1 : bus) {}
+    SPIBusIterator(FilterType filter, uint16_t devid_driver_index, int16_t chipselect = -1, int bus = -1) :
+        _filter(filter), _devid_driver_index(devid_driver_index),
+        _chipselect(chipselect),
+        _bus(filter == FilterType::ExternalBus && bus == -1 ? 1 : bus) {
+    }
+
+    bool next();
 
-	bool next();
+    const px4_spi_bus_t &bus() const {
+        return px4_spi_buses[_index];
+    }
 
-	const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; }
-	spi_drdy_gpio_t DRDYGPIO() const { return px4_spi_buses[_index].devices[_bus_device_index].drdy_gpio; }
+    spi_drdy_gpio_t DRDYGPIO() const {
+        return px4_spi_buses[_index].devices[_bus_device_index].drdy_gpio;
+    }
 
-	uint32_t devid() const { return px4_spi_buses[_index].devices[_bus_device_index].devid; }
+    uint32_t devid() const {
+        return px4_spi_buses[_index].devices[_bus_device_index].devid;
+    }
 
-	int externalBusIndex() const { return _external_bus_counter; }
+    int externalBusIndex() const {
+        return _external_bus_counter;
+    }
 
-	bool external() const { return px4_spi_bus_external(bus()); }
+    bool external() const {
+        return px4_spi_bus_external(bus());
+    }
 
-	int busDeviceIndex() const { return _bus_device_index; }
+    int busDeviceIndex() const {
+        return _bus_device_index;
+    }
 
 private:
-	const FilterType _filter;
-	const uint16_t _devid_driver_index;
-	const int16_t _chipselect;
-	const int _bus;
-	int _index{0};
-	int _external_bus_counter{1};
-	int _bus_device_index{-1};
+    const FilterType _filter;
+    const uint16_t   _devid_driver_index;
+    const int16_t    _chipselect;
+    const int        _bus;
+    int              _index{0};
+    int              _external_bus_counter{1};
+    int              _bus_device_index{-1};
 };
 
 #endif // CONFIG_SPI
diff --git a/pkgs/include/bitmask.h b/pkgs/include/bitmask.h
index 9e6911821e..870d49f13b 100644
--- a/pkgs/include/bitmask.h
+++ b/pkgs/include/bitmask.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 /**
  * @file bitmask.h
@@ -42,111 +19,86 @@
 
 #if defined(__cplusplus) && (defined(__PX4_POSIX) || defined(__PX4_LINUX))
 
-#include<type_traits>
+#   include <type_traits>
 
-template<typename E>
+template <typename E>
 struct enable_bitmask_operators {
-	static const bool enable = false;
+    static const bool enable = false;
 };
 
-namespace px4
-{
+namespace px4 {
 
-#define ENABLE_BIT_OPERATORS(E) \
-	template<> \
-	struct enable_bitmask_operators<E> \
-	{ \
-		static const bool enable = true; \
-	};
+#   define ENABLE_BIT_OPERATORS(E)          \
+       template <>                          \
+       struct enable_bitmask_operators<E> { \
+           static const bool enable = true; \
+       };
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
-operator==(E lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	return static_cast<E>(
-		       static_cast<underlying>(lhs) ==
-		       static_cast<underlying>(rhs)
-	       );
+operator==(E lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    return static_cast<E>(
+        static_cast<underlying>(lhs) == static_cast<underlying>(rhs));
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
-operator~(E lhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	return static_cast<E>(
-		       ~static_cast<underlying>(lhs)
-	       );
+operator~(E lhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    return static_cast<E>(
+        ~static_cast<underlying>(lhs));
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
-operator|(E lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	return static_cast<E>(
-		       static_cast<underlying>(lhs) |
-		       static_cast<underlying>(rhs)
-	       );
+operator|(E lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    return static_cast<E>(
+        static_cast<underlying>(lhs) | static_cast<underlying>(rhs));
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
-operator|=(E &lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	lhs = static_cast<E>(
-		      static_cast<underlying>(lhs) |
-		      static_cast<underlying>(rhs)
-	      );
-	return lhs;
+operator|=(E &lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    lhs = static_cast<E>(
+        static_cast<underlying>(lhs) | static_cast<underlying>(rhs));
+    return lhs;
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
-operator&(E lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	return static_cast<E>(
-		       static_cast<underlying>(lhs) &
-		       static_cast<underlying>(rhs)
-	       );
+operator&(E lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    return static_cast<E>(
+        static_cast<underlying>(lhs) & static_cast<underlying>(rhs));
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
-operator&=(E &lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	lhs = static_cast<E>(
-		      static_cast<underlying>(lhs) &
-		      static_cast<underlying>(rhs)
-	      );
-	return lhs;
+operator&=(E &lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    lhs = static_cast<E>(
+        static_cast<underlying>(lhs) & static_cast<underlying>(rhs));
+    return lhs;
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E>::type
-operator^(E lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	return static_cast<E>(
-		       static_cast<underlying>(lhs) ^
-		       static_cast<underlying>(rhs)
-	       );
+operator^(E lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    return static_cast<E>(
+        static_cast<underlying>(lhs) ^ static_cast<underlying>(rhs));
 }
 
-template<typename E>
+template <typename E>
 typename std::enable_if<enable_bitmask_operators<E>::enable, E &>::type
-operator^=(E &lhs, E rhs)
-{
-	typedef typename std::underlying_type<E>::type underlying;
-	lhs = static_cast<E>(
-		      static_cast<underlying>(lhs) ^
-		      static_cast<underlying>(rhs)
-	      );
-	return lhs;
+operator^=(E &lhs, E rhs) {
+    typedef typename std::underlying_type<E>::type underlying;
+    lhs = static_cast<E>(
+        static_cast<underlying>(lhs) ^ static_cast<underlying>(rhs));
+    return lhs;
 }
 
 } /* namespace px4 */
diff --git a/pkgs/include/constexpr_util.h b/pkgs/include/constexpr_util.h
index 40a1ed43b8..fd0fdbe439 100644
--- a/pkgs/include/constexpr_util.h
+++ b/pkgs/include/constexpr_util.h
@@ -1,46 +1,22 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2019 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
-static inline int _constexpr_assert_failure(const char *msg)
-{
-	// we do 2 things that the compiler will refuse to execute at compile-time
-	// (and therefore trigger a compilation error):
-	// - define a local static variable
-	// - declare the method as non constexpr
-	static int i = 0;
-	return i;
+static inline int _constexpr_assert_failure(const char *msg) {
+    // we do 2 things that the compiler will refuse to execute at compile-time
+    // (and therefore trigger a compilation error):
+    // - define a local static variable
+    // - declare the method as non constexpr
+    static int i = 0;
+    return i;
 }
 
 /**
@@ -54,4 +30,7 @@ static inline int _constexpr_assert_failure(const char *msg)
  *
  * If executed at runtime, it has no effect other than slight runtime overhead.
  */
-#define constexpr_assert(expr, msg) if (!(expr)) { _constexpr_assert_failure(msg); }
+#define constexpr_assert(expr, msg)     \
+    if (!(expr)) {                      \
+        _constexpr_assert_failure(msg); \
+    }
diff --git a/pkgs/muorb/aggregator/mUORBAggregator.cpp b/pkgs/muorb/aggregator/mUORBAggregator.cpp
index 5a331c0973..43e5d485f9 100644
--- a/pkgs/muorb/aggregator/mUORBAggregator.cpp
+++ b/pkgs/muorb/aggregator/mUORBAggregator.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <ulog/log.h>
 #include "mUORBAggregator.hpp"
diff --git a/pkgs/muorb/aggregator/mUORBAggregator.hpp b/pkgs/muorb/aggregator/mUORBAggregator.hpp
index 63bcf5845c..c318d8fcc3 100644
--- a/pkgs/muorb/aggregator/mUORBAggregator.hpp
+++ b/pkgs/muorb/aggregator/mUORBAggregator.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #pragma once
 
@@ -37,55 +14,59 @@
 #include <string.h>
 #include "uORB/uORBCommunicator.hpp"
 
-namespace mUORB
-{
+namespace mUORB {
 
-class Aggregator
-{
+class Aggregator {
 public:
-	typedef int (*sendFuncPtr)(const char *, const uint8_t *, int);
+    typedef int (*sendFuncPtr)(const char *, const uint8_t *, int);
 
-	void RegisterSendHandler(sendFuncPtr func) { sendFunc = func; }
+    void RegisterSendHandler(sendFuncPtr func) {
+        sendFunc = func;
+    }
 
-	void RegisterHandler(uORBCommunicator::IChannelRxHandler *handler) { _RxHandler = handler; }
+    void RegisterHandler(uORBCommunicator::IChannelRxHandler *handler) {
+        _RxHandler = handler;
+    }
 
-	int16_t ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes);
+    int16_t ProcessTransmitTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes);
 
-	void ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes);
+    void ProcessReceivedTopic(const char *topic, const uint8_t *data, uint32_t length_in_bytes);
 
-	int16_t SendData();
+    int16_t SendData();
 
 private:
-	static const bool debugFlag;
+    static const bool debugFlag;
 
-	const std::string topicName = "aggregation";
+    const std::string topicName = "aggregation";
 
-	// Master flag to enable aggregation
-	const bool aggregationEnabled = true;
+    // Master flag to enable aggregation
+    const bool aggregationEnabled = true;
 
-	const uint32_t syncFlag = 0x5A01FF00;
-	const uint32_t syncFlagSize = 4;
-	const uint32_t topicNameLengthSize = 4;
-	const uint32_t dataLengthSize = 4;
-	const uint32_t headerSize = syncFlagSize + topicNameLengthSize + dataLengthSize;
-	static const uint32_t numBuffers = 2;
-	static const uint32_t bufferSize = 2048;
+    const uint32_t        syncFlag            = 0x5A01FF00;
+    const uint32_t        syncFlagSize        = 4;
+    const uint32_t        topicNameLengthSize = 4;
+    const uint32_t        dataLengthSize      = 4;
+    const uint32_t        headerSize          = syncFlagSize + topicNameLengthSize + dataLengthSize;
+    static const uint32_t numBuffers          = 2;
+    static const uint32_t bufferSize          = 2048;
 
-	uint32_t bufferId;
-	uint32_t bufferWriteIndex;
-	uint8_t  buffer[numBuffers][bufferSize];
+    uint32_t bufferId;
+    uint32_t bufferWriteIndex;
+    uint8_t  buffer[numBuffers][bufferSize];
 
-	uORBCommunicator::IChannelRxHandler *_RxHandler;
+    uORBCommunicator::IChannelRxHandler *_RxHandler;
 
-	sendFuncPtr sendFunc;
+    sendFuncPtr sendFunc;
 
-	bool isAggregate(const char *name) { return (strcmp(name, topicName.c_str()) == 0); }
+    bool isAggregate(const char *name) {
+        return (strcmp(name, topicName.c_str()) == 0);
+    }
 
-	bool NewRecordOverflows(const char *messageName, int32_t length);
+    bool NewRecordOverflows(const char *messageName, int32_t length);
 
-	void MoveToNextBuffer();
+    void MoveToNextBuffer();
 
-	void AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data);
+    void AddRecordToBuffer(const char *messageName, int32_t length, const uint8_t *data);
 };
 
-}
+} // namespace mUORB
diff --git a/pkgs/muorb/apps/fc_sensor.h b/pkgs/muorb/apps/fc_sensor.h
index 242a5a708b..1001925a7a 100644
--- a/pkgs/muorb/apps/fc_sensor.h
+++ b/pkgs/muorb/apps/fc_sensor.h
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifndef FC_SENSOR_H
 #define FC_SENSOR_H
@@ -41,27 +18,27 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 
-typedef void (*fc_receive_cb)(const char *topic,
-			      const uint8_t *data,
-			      uint32_t length_in_bytes);
+typedef void (*fc_receive_cb)(const char    *topic,
+                              const uint8_t *data,
+                              uint32_t       length_in_bytes);
 typedef void (*fc_advertise_cb)(const char *topic);
 typedef void (*fc_add_subscription_cb)(const char *topic);
 typedef void (*fc_remove_subscription_cb)(const char *topic);
 
 typedef struct {
-	fc_receive_cb               rx_callback;
-	fc_advertise_cb             ad_callback;
-	fc_add_subscription_cb      sub_callback;
-	fc_remove_subscription_cb   remove_sub_callback;
+    fc_receive_cb             rx_callback;
+    fc_advertise_cb           ad_callback;
+    fc_add_subscription_cb    sub_callback;
+    fc_remove_subscription_cb remove_sub_callback;
 } fc_callbacks;
 
 int fc_sensor_initialize(bool enable_debug_messages, fc_callbacks *callbacks);
 int fc_sensor_advertise(const char *topic);
 int fc_sensor_subscribe(const char *topic);
 int fc_sensor_unsubscribe(const char *topic);
-int fc_sensor_send_data(const char *topic,
-			const uint8_t *data,
-			uint32_t length_in_bytes);
+int fc_sensor_send_data(const char    *topic,
+                        const uint8_t *data,
+                        uint32_t       length_in_bytes);
 #ifdef __cplusplus
 }
 #endif
diff --git a/pkgs/muorb/apps/muorb_main.cpp b/pkgs/muorb/apps/muorb_main.cpp
index d3c4d78cec..0b68fc81ab 100644
--- a/pkgs/muorb/apps/muorb_main.cpp
+++ b/pkgs/muorb/apps/muorb_main.cpp
@@ -1,65 +1,40 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- *   Copyright (c) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <string.h>
 #include "uORBAppsProtobufChannel.hpp"
 #include "uORB/uORBManager.hpp"
 
 extern "C" {
-	__EXPORT int muorb_main(int argc, char *argv[]);
-	__EXPORT int muorb_init();
+__EXPORT int muorb_main(int argc, char *argv[]);
+__EXPORT int muorb_init();
 }
 
 static bool enable_debug = false;
 
-int
-muorb_main(int argc, char *argv[])
-{
-	return muorb_init();
+int muorb_main(int argc, char *argv[]) {
+    return muorb_init();
 }
 
-int
-muorb_init()
-{
-	uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance();
+int muorb_init() {
+    uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance();
 
-	PX4_INFO("Got muorb init command");
+    PX4_INFO("Got muorb init command");
 
-	if (channel && channel->Initialize(enable_debug)) {
-		uORB::Manager::get_instance()->set_uorb_communicator(channel);
+    if (channel && channel->Initialize(enable_debug)) {
+        uORB::Manager::get_instance()->set_uorb_communicator(channel);
 
-		if (channel->Test()) { return OK; }
-	}
+        if (channel->Test()) {
+            return OK;
+        }
+    }
 
-	return -EINVAL;
+    return -EINVAL;
 }
diff --git a/pkgs/muorb/apps/uORBAppsProtobufChannel.cpp b/pkgs/muorb/apps/uORBAppsProtobufChannel.cpp
index 9634165479..a736a07b9c 100644
--- a/pkgs/muorb/apps/uORBAppsProtobufChannel.cpp
+++ b/pkgs/muorb/apps/uORBAppsProtobufChannel.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "uORBAppsProtobufChannel.hpp"
 #include <string.h>
@@ -39,285 +16,296 @@
 bool uORB::AppsProtobufChannel::test_flag = false;
 
 // Initialize the static members
-uORB::AppsProtobufChannel *uORB::AppsProtobufChannel::_InstancePtr = nullptr;
-uORBCommunicator::IChannelRxHandler *uORB::AppsProtobufChannel::_RxHandler = nullptr;
-mUORB::Aggregator uORB::AppsProtobufChannel::_Aggregator;
-std::map<std::string, int> uORB::AppsProtobufChannel::_SlpiSubscriberCache;
-pthread_mutex_t uORB::AppsProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER;
-pthread_mutex_t uORB::AppsProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER;
-bool uORB::AppsProtobufChannel::_Debug = false;
-
-
-void uORB::AppsProtobufChannel::ReceiveCallback(const char *topic,
-		const uint8_t *data,
-		uint32_t length_in_bytes)
-{
-
-	if (_Debug) { PX4_INFO("Got Receive callback for topic %s", topic); }
-
-	if (strcmp(topic, "slpi_debug") == 0) {
-		PX4_INFO("SLPI: %s", (const char *) data);
-
-	} else if (strcmp(topic, "slpi_error") == 0) {
-		PX4_ERR("SLPI: %s", (const char *) data);
-
-	} else if (IS_MUORB_TEST(topic)) {
-		// Validate the test data received
-		bool test_passed = true;
-
-		if (length_in_bytes != MUORB_TEST_DATA_LEN) {
-			test_passed = false;
-
-		} else {
-			for (uint32_t i = 0; i < length_in_bytes; i++) {
-				if ((uint8_t) i != data[i]) {
-					test_passed = false;
-					break;
-				}
-			}
-		}
-
-		if (test_passed) { test_flag = true; }
-
-		return;
-
-	} else if (_RxHandler) {
-		_Aggregator.ProcessReceivedTopic(topic, data, length_in_bytes);
-
-	} else {
-		PX4_ERR("Couldn't handle topic %s in receive callback", topic);
-	}
+uORB::AppsProtobufChannel           *uORB::AppsProtobufChannel::_InstancePtr = nullptr;
+uORBCommunicator::IChannelRxHandler *uORB::AppsProtobufChannel::_RxHandler   = nullptr;
+mUORB::Aggregator                    uORB::AppsProtobufChannel::_Aggregator;
+std::map<std::string, int>           uORB::AppsProtobufChannel::_SlpiSubscriberCache;
+pthread_mutex_t                      uORB::AppsProtobufChannel::_tx_mutex = PTHREAD_MUTEX_INITIALIZER;
+pthread_mutex_t                      uORB::AppsProtobufChannel::_rx_mutex = PTHREAD_MUTEX_INITIALIZER;
+bool                                 uORB::AppsProtobufChannel::_Debug    = false;
+
+void uORB::AppsProtobufChannel::ReceiveCallback(const char    *topic,
+                                                const uint8_t *data,
+                                                uint32_t       length_in_bytes) {
+    if (_Debug) {
+        PX4_INFO("Got Receive callback for topic %s", topic);
+    }
+
+    if (strcmp(topic, "slpi_debug") == 0) {
+        PX4_INFO("SLPI: %s", (const char *)data);
+
+    } else if (strcmp(topic, "slpi_error") == 0) {
+        PX4_ERR("SLPI: %s", (const char *)data);
+
+    } else if (IS_MUORB_TEST(topic)) {
+        // Validate the test data received
+        bool test_passed = true;
+
+        if (length_in_bytes != MUORB_TEST_DATA_LEN) {
+            test_passed = false;
+
+        } else {
+            for (uint32_t i = 0; i < length_in_bytes; i++) {
+                if ((uint8_t)i != data[i]) {
+                    test_passed = false;
+                    break;
+                }
+            }
+        }
+
+        if (test_passed) {
+            test_flag = true;
+        }
+
+        return;
+
+    } else if (_RxHandler) {
+        _Aggregator.ProcessReceivedTopic(topic, data, length_in_bytes);
+
+    } else {
+        PX4_ERR("Couldn't handle topic %s in receive callback", topic);
+    }
 }
 
-void uORB::AppsProtobufChannel::AdvertiseCallback(const char *topic)
-{
-	if (_Debug) { PX4_INFO("Got advertisement callback for topic %s", topic); }
+void uORB::AppsProtobufChannel::AdvertiseCallback(const char *topic) {
+    if (_Debug) {
+        PX4_INFO("Got advertisement callback for topic %s", topic);
+    }
 
-	if (IS_MUORB_TEST(topic)) {
-		test_flag = true;
-		return;
+    if (IS_MUORB_TEST(topic)) {
+        test_flag = true;
+        return;
 
-	} else if (_RxHandler) {
-		_RxHandler->process_remote_topic(topic);
+    } else if (_RxHandler) {
+        _RxHandler->process_remote_topic(topic);
 
-	} else {
-		PX4_ERR("Couldn't handle topic %s in advertise callback", topic);
-	}
+    } else {
+        PX4_ERR("Couldn't handle topic %s in advertise callback", topic);
+    }
 }
 
-void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic)
-{
-	if (_Debug) { PX4_INFO("Got subscription callback for topic %s", topic); }
+void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic) {
+    if (_Debug) {
+        PX4_INFO("Got subscription callback for topic %s", topic);
+    }
 
-	if (IS_MUORB_TEST(topic)) {
-		test_flag = true;
-		return;
+    if (IS_MUORB_TEST(topic)) {
+        test_flag = true;
+        return;
 
-	} else if (_RxHandler) {
-		pthread_mutex_lock(&_rx_mutex);
-		_SlpiSubscriberCache[topic]++;
-		pthread_mutex_unlock(&_rx_mutex);
+    } else if (_RxHandler) {
+        pthread_mutex_lock(&_rx_mutex);
+        _SlpiSubscriberCache[topic]++;
+        pthread_mutex_unlock(&_rx_mutex);
 
-		_RxHandler->process_add_subscription(topic);
+        _RxHandler->process_add_subscription(topic);
 
-	} else {
-		PX4_ERR("Couldn't handle topic %s in subscribe callback", topic);
-	}
+    } else {
+        PX4_ERR("Couldn't handle topic %s in subscribe callback", topic);
+    }
 }
 
-void uORB::AppsProtobufChannel::UnsubscribeCallback(const char *topic)
-{
-	if (_Debug) { PX4_INFO("Got remove subscription callback for topic %s", topic); }
+void uORB::AppsProtobufChannel::UnsubscribeCallback(const char *topic) {
+    if (_Debug) {
+        PX4_INFO("Got remove subscription callback for topic %s", topic);
+    }
 
-	if (IS_MUORB_TEST(topic)) {
-		test_flag = true;
-		return;
+    if (IS_MUORB_TEST(topic)) {
+        test_flag = true;
+        return;
 
-	} else if (_RxHandler) {
-		pthread_mutex_lock(&_rx_mutex);
+    } else if (_RxHandler) {
+        pthread_mutex_lock(&_rx_mutex);
 
-		if (_SlpiSubscriberCache[topic]) { _SlpiSubscriberCache[topic]--; }
+        if (_SlpiSubscriberCache[topic]) {
+            _SlpiSubscriberCache[topic]--;
+        }
 
-		pthread_mutex_unlock(&_rx_mutex);
+        pthread_mutex_unlock(&_rx_mutex);
 
-		_RxHandler->process_remove_subscription(topic);
+        _RxHandler->process_remove_subscription(topic);
 
-	} else {
-		PX4_ERR("Couldn't handle topic %s in unsubscribe callback", topic);
-	}
+    } else {
+        PX4_ERR("Couldn't handle topic %s in unsubscribe callback", topic);
+    }
 }
 
-bool uORB::AppsProtobufChannel::Test(MUORBTestType test_type)
-{
-	int rc = -1;
-	int timeout = 10;
+bool uORB::AppsProtobufChannel::Test(MUORBTestType test_type) {
+    int rc      = -1;
+    int timeout = 10;
 
-	uint8_t test_data[MUORB_TEST_DATA_LEN];
+    uint8_t test_data[MUORB_TEST_DATA_LEN];
 
-	for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) {
-		test_data[i] = i;
-	};
+    for (uint8_t i = 0; i < MUORB_TEST_DATA_LEN; i++) {
+        test_data[i] = i;
+    };
 
-	test_flag = false;
+    test_flag = false;
 
-	switch (test_type) {
-	case ADVERTISE_TEST_TYPE:
-		rc = topic_advertised(muorb_test_topic_name);
-		PX4_INFO("succesfully did ADVERTISE_TEST_TYPE");
-		break;
+    switch (test_type) {
+    case ADVERTISE_TEST_TYPE:
+        rc = topic_advertised(muorb_test_topic_name);
+        PX4_INFO("succesfully did ADVERTISE_TEST_TYPE");
+        break;
 
-	case SUBSCRIBE_TEST_TYPE:
-		rc = add_subscription(muorb_test_topic_name, 1000);
-		PX4_INFO("succesfully did SUBSCRIBE_TEST_TYPE");
-		break;
+    case SUBSCRIBE_TEST_TYPE:
+        rc = add_subscription(muorb_test_topic_name, 1000);
+        PX4_INFO("succesfully did SUBSCRIBE_TEST_TYPE");
+        break;
 
-	case TOPIC_TEST_TYPE:
-		rc = fc_sensor_send_data(muorb_test_topic_name, test_data, MUORB_TEST_DATA_LEN);
-		PX4_INFO("succesfully did TOPIC_TEST_TYPE");
-		break;
+    case TOPIC_TEST_TYPE:
+        rc = fc_sensor_send_data(muorb_test_topic_name, test_data, MUORB_TEST_DATA_LEN);
+        PX4_INFO("succesfully did TOPIC_TEST_TYPE");
+        break;
 
-	case UNSUBSCRIBE_TEST_TYPE:
-		rc = remove_subscription(muorb_test_topic_name);
-		PX4_INFO("succesfully did UNSUBSCRIBE_TEST_TYPE");
-		break;
+    case UNSUBSCRIBE_TEST_TYPE:
+        rc = remove_subscription(muorb_test_topic_name);
+        PX4_INFO("succesfully did UNSUBSCRIBE_TEST_TYPE");
+        break;
 
-	default:
-		PX4_ERR("Unknown test type %d", test_type);
-		break;
-	}
+    default:
+        PX4_ERR("Unknown test type %d", test_type);
+        break;
+    }
 
-	// non zero return code means test failed
-	if (rc) { return false; }
+    // non zero return code means test failed
+    if (rc) {
+        return false;
+    }
 
-	// Wait for test acknowledgement from DSP
-	while ((! test_flag) && (timeout--)) {
-		usleep(10000);
-	}
+    // Wait for test acknowledgement from DSP
+    while ((!test_flag) && (timeout--)) {
+        usleep(10000);
+    }
 
-	if (timeout == -1) {
-		PX4_ERR("Test timed out waiting for response");
-		return false;
-	}
+    if (timeout == -1) {
+        PX4_ERR("Test timed out waiting for response");
+        return false;
+    }
 
-	return true;
+    return true;
 }
 
-bool uORB::AppsProtobufChannel::Test()
-{
-	if (! Test(ADVERTISE_TEST_TYPE)) { return false; }
+bool uORB::AppsProtobufChannel::Test() {
+    if (!Test(ADVERTISE_TEST_TYPE)) {
+        return false;
+    }
 
-	if (! Test(SUBSCRIBE_TEST_TYPE)) { return false; }
+    if (!Test(SUBSCRIBE_TEST_TYPE)) {
+        return false;
+    }
 
-	if (! Test(TOPIC_TEST_TYPE)) { return false; }
+    if (!Test(TOPIC_TEST_TYPE)) {
+        return false;
+    }
 
-	if (! Test(UNSUBSCRIBE_TEST_TYPE)) { return false; }
+    if (!Test(UNSUBSCRIBE_TEST_TYPE)) {
+        return false;
+    }
 
-	PX4_INFO("muorb test passed");
-	return true;
+    PX4_INFO("muorb test passed");
+    return true;
 }
 
-bool uORB::AppsProtobufChannel::Initialize(bool enable_debug)
-{
-	if (! _Initialized) {
-		fc_callbacks cb = { &ReceiveCallback, &AdvertiseCallback,
-				    &SubscribeCallback, &UnsubscribeCallback
-				  };
+bool uORB::AppsProtobufChannel::Initialize(bool enable_debug) {
+    if (!_Initialized) {
+        fc_callbacks cb = {&ReceiveCallback, &AdvertiseCallback,
+                           &SubscribeCallback, &UnsubscribeCallback};
 
-		if (fc_sensor_initialize(enable_debug, &cb) != 0) {
-			if (enable_debug) { PX4_INFO("Warning: muorb protobuf initalize method failed"); }
+        if (fc_sensor_initialize(enable_debug, &cb) != 0) {
+            if (enable_debug) {
+                PX4_INFO("Warning: muorb protobuf initalize method failed");
+            }
 
-		} else {
-			PX4_INFO("muorb protobuf initalize method succeeded");
-			_Initialized = true;
-		}
+        } else {
+            PX4_INFO("muorb protobuf initalize method succeeded");
+            _Initialized = true;
+        }
 
-	} else {
-		PX4_INFO("AppsProtobufChannel already initialized");
-	}
+    } else {
+        PX4_INFO("AppsProtobufChannel already initialized");
+    }
 
-	return true;
+    return true;
 }
 
-int16_t uORB::AppsProtobufChannel::topic_advertised(const char *messageName)
-{
-	if (_Initialized) {
-		if (_Debug) { PX4_INFO("Advertising topic %s to remote side", messageName); }
+int16_t uORB::AppsProtobufChannel::topic_advertised(const char *messageName) {
+    if (_Initialized) {
+        if (_Debug) {
+            PX4_INFO("Advertising topic %s to remote side", messageName);
+        }
 
-		pthread_mutex_lock(&_tx_mutex);
-		int16_t rc = fc_sensor_advertise(messageName);
-		pthread_mutex_unlock(&_tx_mutex);
-		return rc;
-	}
+        pthread_mutex_lock(&_tx_mutex);
+        int16_t rc = fc_sensor_advertise(messageName);
+        pthread_mutex_unlock(&_tx_mutex);
+        return rc;
+    }
 
-	return -1;
+    return -1;
 }
 
-int16_t uORB::AppsProtobufChannel::add_subscription(const char *messageName, int msgRateInHz)
-{
-	// This parameter is unused.
-	(void)(msgRateInHz);
+int16_t uORB::AppsProtobufChannel::add_subscription(const char *messageName, int msgRateInHz) {
+    // This parameter is unused.
+    (void)(msgRateInHz);
 
-	if (_Initialized) {
-		pthread_mutex_lock(&_tx_mutex);
-		int16_t rc = fc_sensor_subscribe(messageName);
-		pthread_mutex_unlock(&_tx_mutex);
-		return rc;
-	}
+    if (_Initialized) {
+        pthread_mutex_lock(&_tx_mutex);
+        int16_t rc = fc_sensor_subscribe(messageName);
+        pthread_mutex_unlock(&_tx_mutex);
+        return rc;
+    }
 
-	return -1;
+    return -1;
 }
 
-int16_t uORB::AppsProtobufChannel::remove_subscription(const char *messageName)
-{
-	if (_Initialized) {
-		pthread_mutex_lock(&_tx_mutex);
-		int16_t rc = fc_sensor_unsubscribe(messageName);
-		pthread_mutex_unlock(&_tx_mutex);
-		return rc;
-	}
+int16_t uORB::AppsProtobufChannel::remove_subscription(const char *messageName) {
+    if (_Initialized) {
+        pthread_mutex_lock(&_tx_mutex);
+        int16_t rc = fc_sensor_unsubscribe(messageName);
+        pthread_mutex_unlock(&_tx_mutex);
+        return rc;
+    }
 
-	return -1;
+    return -1;
 }
 
-int16_t uORB::AppsProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler)
-{
-	_RxHandler = handler;
-	_Aggregator.RegisterHandler(handler);
-	return 0;
+int16_t uORB::AppsProtobufChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) {
+    _RxHandler = handler;
+    _Aggregator.RegisterHandler(handler);
+    return 0;
 }
 
-int16_t uORB::AppsProtobufChannel::send_message(const char *messageName, int length, uint8_t *data)
-{
-	// This is done to slow down high rate debug messages.
-	bool enable_debug = false;
+int16_t uORB::AppsProtobufChannel::send_message(const char *messageName, int length, uint8_t *data) {
+    // This is done to slow down high rate debug messages.
+    bool enable_debug = false;
 
-	if ((_MessageCounter++ % 100) == 0) {
-		enable_debug = true;
-	}
+    if ((_MessageCounter++ % 100) == 0) {
+        enable_debug = true;
+    }
 
-	if (_Initialized) {
-		pthread_mutex_lock(&_rx_mutex);
-		int has_subscribers = _SlpiSubscriberCache[messageName];
-		pthread_mutex_unlock(&_rx_mutex);
+    if (_Initialized) {
+        pthread_mutex_lock(&_rx_mutex);
+        int has_subscribers = _SlpiSubscriberCache[messageName];
+        pthread_mutex_unlock(&_rx_mutex);
 
-		if (has_subscribers) {
-			if (_Debug && enable_debug) {
-				PX4_INFO("Sending data for topic %s", messageName);
-			}
+        if (has_subscribers) {
+            if (_Debug && enable_debug) {
+                PX4_INFO("Sending data for topic %s", messageName);
+            }
 
-			pthread_mutex_lock(&_tx_mutex);
-			int16_t rc = fc_sensor_send_data(messageName, data, length);
-			pthread_mutex_unlock(&_tx_mutex);
-			return rc;
+            pthread_mutex_lock(&_tx_mutex);
+            int16_t rc = fc_sensor_send_data(messageName, data, length);
+            pthread_mutex_unlock(&_tx_mutex);
+            return rc;
 
-		} else {
-			if (_Debug && enable_debug) {
-				PX4_INFO("No subscribers (yet) in %s for topic %s", __FUNCTION__, messageName);
-			}
+        } else {
+            if (_Debug && enable_debug) {
+                PX4_INFO("No subscribers (yet) in %s for topic %s", __FUNCTION__, messageName);
+            }
 
-			return 0;
-		}
-	}
+            return 0;
+        }
+    }
 
-	return -1;
+    return -1;
 }
diff --git a/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp b/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp
index 301c3f325a..1041278a98 100644
--- a/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp
+++ b/pkgs/muorb/apps/uORBAppsProtobufChannel.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #ifndef _uORBAppsProtobufChannel_hpp_
 #define _uORBAppsProtobufChannel_hpp_
@@ -171,7 +148,7 @@ class uORB::AppsProtobufChannel : public uORBCommunicator::IChannel {
     /**
 	 * Class Members
 	 */
-    AppsProtobufChannel(){};
+    AppsProtobufChannel() {};
 
     bool Test(MUORBTestType test_type);
 
diff --git a/pkgs/muorb/slpi/uORBProtobufChannel.cpp b/pkgs/muorb/slpi/uORBProtobufChannel.cpp
index d996a5bd48..684975dc12 100644
--- a/pkgs/muorb/slpi/uORBProtobufChannel.cpp
+++ b/pkgs/muorb/slpi/uORBProtobufChannel.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include "uORBProtobufChannel.hpp"
 #include "uORB/uORBManager.hpp"
diff --git a/pkgs/muorb/slpi/uORBProtobufChannel.hpp b/pkgs/muorb/slpi/uORBProtobufChannel.hpp
index ecd1d02780..d98e8ab7fc 100644
--- a/pkgs/muorb/slpi/uORBProtobufChannel.hpp
+++ b/pkgs/muorb/slpi/uORBProtobufChannel.hpp
@@ -1,35 +1,12 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 #ifndef _uORBProtobufChannel_hpp_
 #define _uORBProtobufChannel_hpp_
 
@@ -43,23 +20,20 @@
 #include "uORB/uORBCommunicator.hpp"
 #include "mUORBAggregator.hpp"
 
-namespace uORB
-{
+namespace uORB {
 class ProtobufChannel;
 }
 
-class uORB::ProtobufChannel : public uORBCommunicator::IChannel
-{
+class uORB::ProtobufChannel : public uORBCommunicator::IChannel {
 public:
-	/**
+    /**
 	 * static method to get the IChannel Implementor.
 	 */
-	static uORB::ProtobufChannel *GetInstance()
-	{
-		return &(_Instance);
-	}
+    static uORB::ProtobufChannel *GetInstance() {
+        return &(_Instance);
+    }
 
-	/**
+    /**
 	 * @brief Interface to notify the remote entity of a topic being advertised.
 	 *
 	 * @param messageName
@@ -70,9 +44,9 @@ class uORB::ProtobufChannel : public uORBCommunicator::IChannel
 	 * 		Note: This does not mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	int16_t topic_advertised(const char *messageName);
+    int16_t topic_advertised(const char *messageName);
 
-	/**
+    /**
 	 * @brief Interface to notify the remote entity of interest of a
 	 * subscription for a message.
 	 *
@@ -86,9 +60,9 @@ class uORB::ProtobufChannel : public uORBCommunicator::IChannel
 	 * 		Note: This does not mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	int16_t add_subscription(const char *messageName, int32_t msgRateInHz);
+    int16_t add_subscription(const char *messageName, int32_t msgRateInHz);
 
-	/**
+    /**
 	 * @brief Interface to notify the remote entity of removal of a subscription
 	 *
 	 * @param messageName
@@ -99,14 +73,14 @@ class uORB::ProtobufChannel : public uORBCommunicator::IChannel
 	 * 		Note: This does not necessarily mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	int16_t remove_subscription(const char *messageName);
+    int16_t remove_subscription(const char *messageName);
 
-	/**
+    /**
 	 * Register Message Handler.  This is internal for the IChannel implementer*
 	 */
-	int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler);
+    int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler);
 
-	/**
+    /**
 	 * @brief Sends the data message over the communication link.
 	 * @param messageName
 	 * 	This represents the uORB message name; This message name should be
@@ -120,100 +94,97 @@ class uORB::ProtobufChannel : public uORBCommunicator::IChannel
 	 * 		Note: This does not mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	int16_t send_message(const char *messageName, int32_t length, uint8_t *data);
+    int16_t send_message(const char *messageName, int32_t length, uint8_t *data);
 
-	uORBCommunicator::IChannelRxHandler *GetRxHandler()
-	{
-		return _RxHandler;
-	}
+    uORBCommunicator::IChannelRxHandler *GetRxHandler() {
+        return _RxHandler;
+    }
 
-	void RegisterSendHandler(mUORB::Aggregator::sendFuncPtr func)
-	{
-		_Aggregator.RegisterSendHandler(func);
-	}
+    void RegisterSendHandler(mUORB::Aggregator::sendFuncPtr func) {
+        _Aggregator.RegisterSendHandler(func);
+    }
 
-	void AddRemoteSubscriber(const std::string &messageName)
-	{
-		pthread_mutex_lock(&_rx_mutex);
-		_AppsSubscriberCache[messageName]++;
-		pthread_mutex_unlock(&_rx_mutex);
-	}
+    void AddRemoteSubscriber(const std::string &messageName) {
+        pthread_mutex_lock(&_rx_mutex);
+        _AppsSubscriberCache[messageName]++;
+        pthread_mutex_unlock(&_rx_mutex);
+    }
 
-	void RemoveRemoteSubscriber(const std::string &messageName)
-	{
-		pthread_mutex_lock(&_rx_mutex);
+    void RemoveRemoteSubscriber(const std::string &messageName) {
+        pthread_mutex_lock(&_rx_mutex);
 
-		if (_AppsSubscriberCache[messageName]) {
-			_AppsSubscriberCache[messageName]--;
-		}
+        if (_AppsSubscriberCache[messageName]) {
+            _AppsSubscriberCache[messageName]--;
+        }
 
-		pthread_mutex_unlock(&_rx_mutex);
-	}
+        pthread_mutex_unlock(&_rx_mutex);
+    }
 
-	bool DebugEnabled()	{ return _debug; }
+    bool DebugEnabled() {
+        return _debug;
+    }
 
-	void SendAggregateData()
-	{
-		pthread_mutex_lock(&_tx_mutex);
-		_Aggregator.SendData();
-		pthread_mutex_unlock(&_tx_mutex);
-	}
+    void SendAggregateData() {
+        pthread_mutex_lock(&_tx_mutex);
+        _Aggregator.SendData();
+        pthread_mutex_unlock(&_tx_mutex);
+    }
 
 private:
-	/**
+    /**
 	 * Data Members
 	 */
-	static uORB::ProtobufChannel                _Instance;
-	static uORBCommunicator::IChannelRxHandler *_RxHandler;
-	static mUORB::Aggregator					_Aggregator;
-	static std::map<std::string, int>           _AppsSubscriberCache;
-	static pthread_mutex_t                      _tx_mutex;
-	static pthread_mutex_t                      _rx_mutex;
-	static bool                                 _debug;
-
-	/**
+    static uORB::ProtobufChannel                _Instance;
+    static uORBCommunicator::IChannelRxHandler *_RxHandler;
+    static mUORB::Aggregator                    _Aggregator;
+    static std::map<std::string, int>           _AppsSubscriberCache;
+    static pthread_mutex_t                      _tx_mutex;
+    static pthread_mutex_t                      _rx_mutex;
+    static bool                                 _debug;
+
+    /**
 	 * Class Members
 	 */
-	ProtobufChannel() {};
+    ProtobufChannel() {};
 };
 
 // TODO: This has to be defined in the slpi_proc build and in the PX4 build.
 // Make it accessible from one file to both builds.
 typedef struct {
-	int (*advertise_func_ptr)(const char *topic_name);
-	int (*subscribe_func_ptr)(const char *topic_name);
-	int (*unsubscribe_func_ptr)(const char *topic_name);
-	int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes);
-	// device::SPI::_config_spi_bus_func_t config_spi_bus;
-	// device::SPI::_spi_transfer_func_t spi_transfer;
-	int (*_config_spi_bus_func_t)();
-	int (*_spi_transfer_func_t)(int, const uint8_t *, uint8_t *, const unsigned);
-	// device::I2C::_config_i2c_bus_func_t config_i2c_bus;
-	// device::I2C::_set_i2c_address_func_t set_i2c_address;
-	// device::I2C::_i2c_transfer_func_t i2c_transfer;
-	int (*_config_i2c_bus_func_t)(uint8_t, uint8_t, uint32_t);
-	int (*_set_i2c_address_func_t)(int, uint8_t);
-	int (*_i2c_transfer_func_t)(int, const uint8_t *, const unsigned, uint8_t *, const unsigned);
-	// open_uart_func_t open_uart_func;
-	// write_uart_func_t write_uart_func;
-	// read_uart_func_t read_uart_func;
-	int (*open_uart_func_t)(uint8_t, speed_t);
-	int (*write_uart_func_t)(int, const void *, size_t);
-	int (*read_uart_func_t)(int,  void *, size_t);
-	int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg);
+    int (*advertise_func_ptr)(const char *topic_name);
+    int (*subscribe_func_ptr)(const char *topic_name);
+    int (*unsubscribe_func_ptr)(const char *topic_name);
+    int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes);
+    // device::SPI::_config_spi_bus_func_t config_spi_bus;
+    // device::SPI::_spi_transfer_func_t spi_transfer;
+    int (*_config_spi_bus_func_t)();
+    int (*_spi_transfer_func_t)(int, const uint8_t *, uint8_t *, const unsigned);
+    // device::I2C::_config_i2c_bus_func_t config_i2c_bus;
+    // device::I2C::_set_i2c_address_func_t set_i2c_address;
+    // device::I2C::_i2c_transfer_func_t i2c_transfer;
+    int (*_config_i2c_bus_func_t)(uint8_t, uint8_t, uint32_t);
+    int (*_set_i2c_address_func_t)(int, uint8_t);
+    int (*_i2c_transfer_func_t)(int, const uint8_t *, const unsigned, uint8_t *, const unsigned);
+    // open_uart_func_t open_uart_func;
+    // write_uart_func_t write_uart_func;
+    // read_uart_func_t read_uart_func;
+    int (*open_uart_func_t)(uint8_t, speed_t);
+    int (*write_uart_func_t)(int, const void *, size_t);
+    int (*read_uart_func_t)(int, void *, size_t);
+    int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg);
 } fc_func_ptrs;
 
 extern "C" {
 
-	int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) __EXPORT;
+int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) __EXPORT;
 
-	int px4muorb_topic_advertised(const char *name) __EXPORT;
+int px4muorb_topic_advertised(const char *name) __EXPORT;
 
-	int px4muorb_add_subscriber(const char *name) __EXPORT;
+int px4muorb_add_subscriber(const char *name) __EXPORT;
 
-	int px4muorb_remove_subscriber(const char *name) __EXPORT;
+int px4muorb_remove_subscriber(const char *name) __EXPORT;
 
-	int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
+int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
 }
 
 #endif // _uORBProtobufChannel_hpp_
diff --git a/pkgs/muorb/test/MUORBTest.cpp b/pkgs/muorb/test/MUORBTest.cpp
index 61921cedca..1ddeb71c37 100644
--- a/pkgs/muorb/test/MUORBTest.cpp
+++ b/pkgs/muorb/test/MUORBTest.cpp
@@ -1,35 +1,12 @@
-/****************************************************************************
- *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 // Definition of common test name between DSP and CPU
 char muorb_test_topic_name[] = "muorb_test";
diff --git a/pkgs/muorb/test/MUORBTest.hpp b/pkgs/muorb/test/MUORBTest.hpp
index 83da6a8fb3..ab7da92d1b 100644
--- a/pkgs/muorb/test/MUORBTest.hpp
+++ b/pkgs/muorb/test/MUORBTest.hpp
@@ -1,48 +1,25 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2022 ModalAI, Inc. 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 // These are the test types for each of the operations that can happen with muorb
 typedef enum {
-	ADVERTISE_TEST_TYPE,
-	SUBSCRIBE_TEST_TYPE,
-	TOPIC_TEST_TYPE,
-	UNSUBSCRIBE_TEST_TYPE
+    ADVERTISE_TEST_TYPE,
+    SUBSCRIBE_TEST_TYPE,
+    TOPIC_TEST_TYPE,
+    UNSUBSCRIBE_TEST_TYPE
 } MUORBTestType;
 
 // Common test name between DSP and CPU
 extern char muorb_test_topic_name[];
 
 // Check if topic is muorb test marker
-#define IS_MUORB_TEST(x) (strcmp(x, muorb_test_topic_name) == 0)
+#define IS_MUORB_TEST(x)    (strcmp(x, muorb_test_topic_name) == 0)
 
 #define MUORB_TEST_DATA_LEN 8
diff --git a/pkgs/param/module_params.c b/pkgs/param/module_params.c
index d02d8e3df0..fa82a979a9 100644
--- a/pkgs/param/module_params.c
+++ b/pkgs/param/module_params.c
@@ -1,6258 +1,6287 @@
 
 /**
- * Airframe selection
+ * Enable Gripper actuation in Payload Deliverer
  *
- * Defines which mixer implementation to use.
- * Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.
- * 
- * 'Custom' should only be used if noting else can be used.
  * 
  *
- * @group Geometry
- * @value 0 Multirotor
- * @value 1 Fixed-wing
- * @value 2 Standard VTOL
- * @value 3 Tiltrotor VTOL
- * @value 4 Tailsitter VTOL
- * @value 5 Rover (Ackermann)
- * @value 6 Rover (Differential)
- * @value 7 Motors (6DOF)
- * @value 8 Multirotor with Tilt
- * @value 9 Custom
- * @value 10 Helicopter
+ * @group Payload Deliverer
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
+PARAM_DEFINE_INT32(PD_GRIPPER_EN, 0);
 
 /**
- * Control allocation method
+ * Type of Gripper (Servo, etc.)
  *
- * Selects the algorithm and desaturation method.
- * If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
  * 
  *
- * @group Geometry
- * @value 0 Pseudo-inverse with output clipping
- * @value 1 Pseudo-inverse with sequential desaturation technique
- * @value 2 Automatic
+ * @group Payload Deliverer
+ * @value -1 Undefined
+ * @value 0 Servo
+ * @min -1
+ * @max 0
  */
-PARAM_DEFINE_INT32(CA_METHOD, 2);
+PARAM_DEFINE_INT32(PD_GRIPPER_TYPE, 0);
 
 /**
- * Bidirectional/Reversible motors
+ * Timeout for successful gripper actuation acknowledgement
  *
- * Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well.
+ * Maximum time Gripper will wait while the successful griper actuation isn't recognised.
+ * If the gripper has no feedback sensor, it will simply wait for
+ * this time before considering gripper actuation successful and publish a
+ * 'VehicleCommandAck' signaling successful gripper action
  * 
  *
- * @group Geometry
- * @bit 0 Motor 1
- * @bit 1 Motor 2
- * @bit 2 Motor 3
- * @bit 3 Motor 4
- * @bit 4 Motor 5
- * @bit 5 Motor 6
- * @bit 6 Motor 7
- * @bit 7 Motor 8
- * @bit 8 Motor 9
- * @bit 9 Motor 10
- * @bit 10 Motor 11
- * @bit 11 Motor 12
+ * @group Payload Deliverer
  * @min 0
- * @max 4095
+ * @unit s
  */
-PARAM_DEFINE_INT32(CA_R_REV, 0);
+PARAM_DEFINE_FLOAT(PD_GRIPPER_TO, 3);
 
 /**
- * Motor 0 slew rate limit
+ * Battery 1 voltage divider (V divider)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This is the divider from battery 1 voltage to ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 8
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R0_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_V_DIV, -1.0);
 
 /**
- * Motor 1 slew rate limit
+ * Battery 2 voltage divider (V divider)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This is the divider from battery 2 voltage to ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 8
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R1_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_V_DIV, -1.0);
 
 /**
- * Motor 2 slew rate limit
+ * Battery 1 current per volt (A/V)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * The voltage seen by the ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 8
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R2_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_A_PER_V, -1.0);
 
 /**
- * Motor 3 slew rate limit
+ * Battery 2 current per volt (A/V)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * The voltage seen by the ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 8
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R3_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_A_PER_V, -1.0);
 
 /**
- * Motor 4 slew rate limit
+ * Battery 1 Voltage ADC Channel
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This parameter specifies the ADC channel used to monitor voltage of main power battery.
+ * A value of -1 means to use the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R4_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT1_V_CHANNEL, -1);
 
 /**
- * Motor 5 slew rate limit
+ * Battery 2 Voltage ADC Channel
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This parameter specifies the ADC channel used to monitor voltage of main power battery.
+ * A value of -1 means to use the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R5_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT2_V_CHANNEL, -1);
 
 /**
- * Motor 6 slew rate limit
+ * Battery 1 Current ADC Channel
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This parameter specifies the ADC channel used to monitor current of main power battery.
+ * A value of -1 means to use the board default.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.01
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R6_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT1_I_CHANNEL, -1);
 
 /**
- * Motor 7 slew rate limit
+ * Battery 2 Current ADC Channel
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
+ * This parameter specifies the ADC channel used to monitor current of main power battery.
+ * A value of -1 means to use the board default.
  * 
- * Zero means that slew rate limiting is disabled.
+ *
+ * @group Battery Calibration
+ * @reboot_required True
+ */
+PARAM_DEFINE_INT32(BAT2_I_CHANNEL, -1);
+
+/**
+ * Empty cell voltage (5C load)
+ *
+ * Defines the voltage where a single cell of battery 1 is considered empty.
+ * The voltage should be chosen before the steep dropoff to 2.8V. A typical
+ * lithium battery can only be discharged down to 10% before it drops off
+ * to a voltage level damaging the cells.
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
  * @increment 0.01
- * @min 0
- * @max 10
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R7_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_V_EMPTY, 3.6);
 
 /**
- * Motor 8 slew rate limit
+ * Empty cell voltage (5C load)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the voltage where a single cell of battery 1 is considered empty.
+ * The voltage should be chosen before the steep dropoff to 2.8V. A typical
+ * lithium battery can only be discharged down to 10% before it drops off
+ * to a voltage level damaging the cells.
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
  * @increment 0.01
- * @min 0
- * @max 10
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R8_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_V_EMPTY, 3.6);
 
 /**
- * Motor 9 slew rate limit
+ * Full cell voltage (5C load)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the voltage where a single cell of battery 1 is considered full
+ * under a mild load. This will never be the nominal voltage of 4.2V
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
  * @increment 0.01
- * @min 0
- * @max 10
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R9_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_V_CHARGED, 4.05);
 
 /**
- * Motor 10 slew rate limit
+ * Full cell voltage (5C load)
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the voltage where a single cell of battery 1 is considered full
+ * under a mild load. This will never be the nominal voltage of 4.2V
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
  * @increment 0.01
- * @min 0
- * @max 10
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R10_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_V_CHARGED, 4.05);
 
 /**
- * Motor 11 slew rate limit
+ * Voltage drop per cell on full throttle
  *
- * Minimum time allowed for the motor input signal to pass through
- * the full output range. A value x means that the motor signal
- * can only go from 0 to 1 in minimum x seconds (in case of
- * reversible motors, the range is -1 to 1).
- * 
- * Zero means that slew rate limiting is disabled.
+ * This implicitly defines the internal resistance
+ * to maximum current ratio for battery 1 and assumes linearity.
+ * A good value to use is the difference between the
+ * 5C and 20-25C load. Not used if BAT1_R_INTERNAL is
+ * set.
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
  * @increment 0.01
- * @min 0
- * @max 10
+ * @min 0.07
+ * @max 0.5
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_R11_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_V_LOAD_DROP, 0.1);
 
 /**
- * Servo 0 slew rate limit
+ * Voltage drop per cell on full throttle
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * This implicitly defines the internal resistance
+ * to maximum current ratio for battery 1 and assumes linearity.
+ * A good value to use is the difference between the
+ * 5C and 20-25C load. Not used if BAT2_R_INTERNAL is
+ * set.
  * 
  *
- * @group Geometry
+ * @group Battery Calibration
  * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @increment 0.01
+ * @min 0.07
+ * @max 0.5
+ * @unit V
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV0_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_V_LOAD_DROP, 0.1);
 
 /**
- * Servo 1 slew rate limit
+ * Explicitly defines the per cell internal resistance for battery 1
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * If non-negative, then this will be used in place of
+ * BAT1_V_LOAD_DROP for all calculations.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 4
+ * @increment 0.0005
+ * @min -1.0
+ * @max 0.2
+ * @unit Ohm
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV1_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, 0.005);
 
 /**
- * Servo 2 slew rate limit
+ * Explicitly defines the per cell internal resistance for battery 2
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * If non-negative, then this will be used in place of
+ * BAT2_V_LOAD_DROP for all calculations.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 4
+ * @increment 0.0005
+ * @min -1.0
+ * @max 0.2
+ * @unit Ohm
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV2_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_R_INTERNAL, 0.005);
 
 /**
- * Servo 3 slew rate limit
+ * Number of cells for battery 1.
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the number of cells the attached battery consists of.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @value 1 1S Battery
+ * @value 2 2S Battery
+ * @value 3 3S Battery
+ * @value 4 4S Battery
+ * @value 5 5S Battery
+ * @value 6 6S Battery
+ * @value 7 7S Battery
+ * @value 8 8S Battery
+ * @value 9 9S Battery
+ * @value 10 10S Battery
+ * @value 11 11S Battery
+ * @value 12 12S Battery
+ * @value 13 13S Battery
+ * @value 14 14S Battery
+ * @value 15 15S Battery
+ * @value 16 16S Battery
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV3_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT1_N_CELLS, 0);
 
 /**
- * Servo 4 slew rate limit
+ * Number of cells for battery 2.
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the number of cells the attached battery consists of.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @value 1 1S Battery
+ * @value 2 2S Battery
+ * @value 3 3S Battery
+ * @value 4 4S Battery
+ * @value 5 5S Battery
+ * @value 6 6S Battery
+ * @value 7 7S Battery
+ * @value 8 8S Battery
+ * @value 9 9S Battery
+ * @value 10 10S Battery
+ * @value 11 11S Battery
+ * @value 12 12S Battery
+ * @value 13 13S Battery
+ * @value 14 14S Battery
+ * @value 15 15S Battery
+ * @value 16 16S Battery
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV4_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT2_N_CELLS, 0);
 
 /**
- * Servo 5 slew rate limit
+ * Battery 1 capacity.
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the capacity of battery 1 in mAh.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 0
+ * @increment 50
+ * @min -1.0
+ * @max 100000
+ * @unit mAh
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV5_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT1_CAPACITY, -1.0);
 
 /**
- * Servo 6 slew rate limit
+ * Battery 2 capacity.
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * Defines the capacity of battery 2 in mAh.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @decimal 0
+ * @increment 50
+ * @min -1.0
+ * @max 100000
+ * @unit mAh
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV6_SLEW, 0.0);
+PARAM_DEFINE_FLOAT(BAT2_CAPACITY, -1.0);
 
 /**
- * Servo 7 slew rate limit
+ * Battery 1 monitoring source.
  *
- * Minimum time allowed for the servo input signal to pass through
- * the full output range. A value x means that the servo signal
- * can only go from -1 to 1 in minimum x seconds.
- * 
- * Zero means that slew rate limiting is disabled.
+ * This parameter controls the source of battery data. The value 'Power Module'
+ * means that measurements are expected to come from a power module. If the value is set to
+ * 'External' then the system expects to receive mavlink battery status messages.
+ * If the value is set to 'ESCs', the battery information are taken from the esc_status message.
+ * This requires the ESC to provide both voltage as well as current.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.05
- * @min 0
- * @max 10
+ * @group Battery Calibration
+ * @value -1 Disabled
+ * @value 0 Power Module
+ * @value 1 External
+ * @value 2 ESCs
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_SV7_SLEW, 0.0);
+PARAM_DEFINE_INT32(BAT1_SOURCE, 0);
 
 /**
- * Total number of rotors
+ * Battery 2 monitoring source.
  *
+ * This parameter controls the source of battery data. The value 'Power Module'
+ * means that measurements are expected to come from a power module. If the value is set to
+ * 'External' then the system expects to receive mavlink battery status messages.
+ * If the value is set to 'ESCs', the battery information are taken from the esc_status message.
+ * This requires the ESC to provide both voltage as well as current.
  * 
  *
- * @group Geometry
- * @value 0 0
- * @value 1 1
- * @value 2 2
- * @value 3 3
- * @value 4 4
- * @value 5 5
- * @value 6 6
- * @value 7 7
- * @value 8 8
- * @value 9 9
- * @value 10 10
- * @value 11 11
- * @value 12 12
+ * @group Battery Calibration
+ * @value -1 Disabled
+ * @value 0 Power Module
+ * @value 1 External
+ * @value 2 ESCs
+ * @reboot_required True
  */
-PARAM_DEFINE_INT32(CA_ROTOR_COUNT, 0);
+PARAM_DEFINE_INT32(BAT2_SOURCE, -1);
 
 /**
- * Position of rotor 0 along X body axis
+ * uXRCE-DDS domain ID
  *
- * 
+ * uXRCE-DDS domain ID
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group UXRCE-DDS Client
+ * @category System
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_PX, 0.0);
+PARAM_DEFINE_INT32(UXRCE_DDS_DOM_ID, 0);
 
 /**
- * Position of rotor 1 along X body axis
+ * uXRCE-DDS Session key
  *
+ * uXRCE-DDS key, must be different from zero.
+ * In a single agent - multi client configuration, each client
+ * must have a unique session key.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group UXRCE-DDS Client
+ * @category System
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_PX, 0.0);
+PARAM_DEFINE_INT32(UXRCE_DDS_KEY, 1);
 
 /**
- * Position of rotor 2 along X body axis
+ * uXRCE-DDS UDP Port
  *
+ * If ethernet enabled and selected as configuration for uXRCE-DDS,
+ * selected udp port will be set and used.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group UXRCE-DDS Client
+ * @min 0
+ * @max 65535
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_PX, 0.0);
+PARAM_DEFINE_INT32(UXRCE_DDS_PRT, 8888);
 
 /**
- * Position of rotor 3 along X body axis
+ * uXRCE-DDS Agent IP address
  *
+ * If ethernet enabled and selected as configuration for uXRCE-DDS,
+ * selected Agent IP address will be set and used.
+ * Decimal dot notation is not supported. IP address must be provided
+ * in int32 format. For example, 192.168.1.2 is mapped to -1062731518;
+ * 127.0.0.1 is mapped to 2130706433.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group UXRCE-DDS Client
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_PX, 0.0);
+PARAM_DEFINE_INT32(UXRCE_DDS_AG_IP, 2130706433);
 
 /**
- * Position of rotor 4 along X body axis
+ * MAVLink Config for instance 0
  *
+ * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Disabled
+ * @value 1 Uart1
+ * @value 2 Uart2
+ * @value 3 Uart3
+ * @value 4 Uart4
+ * @value 5 Uart5
+ * @value 6 Uart6
+ * @value 7 Uart7
+ * @value 8 Uart8
+ * @value 20 UBS1
+ * @value 21 USB2
+ * @value 30 UDP
+ * @value 31 TCP
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_0_CONFIG, 0);
 
 /**
- * Position of rotor 5 along X body axis
+ * MAVLink Config for instance 1
  *
+ * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Disabled
+ * @value 1 Uart1
+ * @value 2 Uart2
+ * @value 3 Uart3
+ * @value 4 Uart4
+ * @value 5 Uart5
+ * @value 6 Uart6
+ * @value 7 Uart7
+ * @value 8 Uart8
+ * @value 20 UBS1
+ * @value 21 USB2
+ * @value 30 UDP
+ * @value 31 TCP
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_1_CONFIG, 0);
 
 /**
- * Position of rotor 6 along X body axis
+ * MAVLink Config for instance 2
  *
+ * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Disabled
+ * @value 1 Uart1
+ * @value 2 Uart2
+ * @value 3 Uart3
+ * @value 4 Uart4
+ * @value 5 Uart5
+ * @value 6 Uart6
+ * @value 7 Uart7
+ * @value 8 Uart8
+ * @value 20 UBS1
+ * @value 21 USB2
+ * @value 30 UDP
+ * @value 31 TCP
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_2_CONFIG, 0);
 
 /**
- * Position of rotor 7 along X body axis
+ * MAVLink Mode for instance 0
  *
+ * The MAVLink Mode defines the set of streamed messages (for example the
+ * vehicle's attitude) and their sending rates.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Normal
+ * @value 1 Custom
+ * @value 2 Onboard
+ * @value 3 OSD
+ * @value 4 Magic
+ * @value 5 Config
+ * @value 7 Minimal
+ * @value 8 External Vision
+ * @value 10 Gimbal
+ * @value 11 Onboard Low Bandwidth
+ * @value 12 uAvionix
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_0_MODE, 0);
 
 /**
- * Position of rotor 8 along X body axis
+ * MAVLink Mode for instance 1
  *
+ * The MAVLink Mode defines the set of streamed messages (for example the
+ * vehicle's attitude) and their sending rates.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Normal
+ * @value 1 Custom
+ * @value 2 Onboard
+ * @value 3 OSD
+ * @value 4 Magic
+ * @value 5 Config
+ * @value 7 Minimal
+ * @value 8 External Vision
+ * @value 10 Gimbal
+ * @value 11 Onboard Low Bandwidth
+ * @value 12 uAvionix
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_1_MODE, 2);
 
 /**
- * Position of rotor 9 along X body axis
+ * MAVLink Mode for instance 2
  *
+ * The MAVLink Mode defines the set of streamed messages (for example the
+ * vehicle's attitude) and their sending rates.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Normal
+ * @value 1 Custom
+ * @value 2 Onboard
+ * @value 3 OSD
+ * @value 4 Magic
+ * @value 5 Config
+ * @value 7 Minimal
+ * @value 8 External Vision
+ * @value 10 Gimbal
+ * @value 11 Onboard Low Bandwidth
+ * @value 12 uAvionix
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_2_MODE, 0);
 
 /**
- * Position of rotor 10 along X body axis
+ * Maximum MAVLink sending rate for instance 0
  *
+ * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
+ * If the configured streams exceed the maximum rate, the sending rate of
+ * each stream is automatically decreased.
+ * 
+ * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
+ * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
+ * 8N1-configured links).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @min 0
+ * @unit B/s
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_0_RATE, 1200);
 
 /**
- * Position of rotor 11 along X body axis
+ * Maximum MAVLink sending rate for instance 1
  *
+ * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
+ * If the configured streams exceed the maximum rate, the sending rate of
+ * each stream is automatically decreased.
+ * 
+ * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
+ * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
+ * 8N1-configured links).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @min 0
+ * @unit B/s
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_PX, 0.0);
+PARAM_DEFINE_INT32(MAV_1_RATE, 0);
 
 /**
- * Position of rotor 0 along Y body axis
+ * Maximum MAVLink sending rate for instance 2
  *
+ * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
+ * If the configured streams exceed the maximum rate, the sending rate of
+ * each stream is automatically decreased.
+ * 
+ * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
+ * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
+ * 8N1-configured links).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @min 0
+ * @unit B/s
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_2_RATE, 0);
 
 /**
- * Position of rotor 1 along Y body axis
+ * Enable MAVLink Message forwarding for instance 0
  *
+ * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
+ * message is either broadcast or the target is not the autopilot.
+ * 
+ * This allows for example a GCS to talk to a camera that is connected to the
+ * autopilot via MAVLink (on a different link than the GCS).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_0_FORWARD, 1);
 
 /**
- * Position of rotor 2 along Y body axis
+ * Enable MAVLink Message forwarding for instance 1
  *
+ * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
+ * message is either broadcast or the target is not the autopilot.
+ * 
+ * This allows for example a GCS to talk to a camera that is connected to the
+ * autopilot via MAVLink (on a different link than the GCS).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_1_FORWARD, 0);
 
 /**
- * Position of rotor 3 along Y body axis
+ * Enable MAVLink Message forwarding for instance 2
  *
+ * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
+ * message is either broadcast or the target is not the autopilot.
+ * 
+ * This allows for example a GCS to talk to a camera that is connected to the
+ * autopilot via MAVLink (on a different link than the GCS).
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_2_FORWARD, 0);
 
 /**
- * Position of rotor 4 along Y body axis
+ * Enable software throttling of mavlink on instance 0
  *
+ * If enabled, MAVLink messages will be throttled according to
+ * `txbuf` field reported by radio_status.
+ * 
+ * Requires a radio to send the mavlink message RADIO_STATUS.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_0_RADIO_CTL, 1);
 
 /**
- * Position of rotor 5 along Y body axis
+ * Enable software throttling of mavlink on instance 1
  *
+ * If enabled, MAVLink messages will be throttled according to
+ * `txbuf` field reported by radio_status.
+ * 
+ * Requires a radio to send the mavlink message RADIO_STATUS.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_1_RADIO_CTL, 1);
 
 /**
- * Position of rotor 6 along Y body axis
+ * Enable software throttling of mavlink on instance 2
  *
+ * If enabled, MAVLink messages will be throttled according to
+ * `txbuf` field reported by radio_status.
+ * 
+ * Requires a radio to send the mavlink message RADIO_STATUS.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @boolean
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_2_RADIO_CTL, 1);
 
 /**
- * Position of rotor 7 along Y body axis
+ * MAVLink Network Port for instance 0
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 0,
+ * selected udp port will be set and used in MAVLink instance 0.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_0_UDP_PRT, 14556);
 
 /**
- * Position of rotor 8 along Y body axis
+ * MAVLink Network Port for instance 1
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 1,
+ * selected udp port will be set and used in MAVLink instance 1.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_1_UDP_PRT, 0);
 
 /**
- * Position of rotor 9 along Y body axis
+ * MAVLink Network Port for instance 2
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 2,
+ * selected udp port will be set and used in MAVLink instance 2.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_2_UDP_PRT, 0);
 
 /**
- * Position of rotor 10 along Y body axis
+ * MAVLink Remote IP for instance 0
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 0,
+ * selected remote IP will be set and used in MAVLink instance 0.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_0_REMOTE_IP, 0);
 
 /**
- * Position of rotor 11 along Y body axis
+ * MAVLink Remote IP for instance 1
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 1,
+ * selected remote IP will be set and used in MAVLink instance 1.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_PY, 0.0);
+PARAM_DEFINE_INT32(MAV_1_REMOTE_IP, 0);
 
 /**
- * Position of rotor 0 along Z body axis
+ * MAVLink Remote IP for instance 2
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 2,
+ * selected remote IP will be set and used in MAVLink instance 2.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_2_REMOTE_IP, 0);
 
 /**
- * Position of rotor 1 along Z body axis
+ * MAVLink Remote Port for instance 0
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 0,
+ * selected remote port will be set and used in MAVLink instance 0.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_0_REMOTE_PRT, 14550);
 
 /**
- * Position of rotor 2 along Z body axis
+ * MAVLink Remote Port for instance 1
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 1,
+ * selected remote port will be set and used in MAVLink instance 1.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_1_REMOTE_PRT, 0);
 
 /**
- * Position of rotor 3 along Z body axis
+ * MAVLink Remote Port for instance 2
  *
+ * If ethernet enabled and selected as configuration for MAVLink instance 2,
+ * selected remote port will be set and used in MAVLink instance 2.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_2_REMOTE_PRT, 0);
 
 /**
- * Position of rotor 4 along Z body axis
+ * Broadcast heartbeats on local network for MAVLink instance 0
  *
+ * This allows a ground control station to automatically find the drone
+ * on the local network.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Never broadcast
+ * @value 1 Always broadcast
+ * @value 2 Only multicast
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_0_BROADCAST, 1);
 
 /**
- * Position of rotor 5 along Z body axis
+ * Broadcast heartbeats on local network for MAVLink instance 1
  *
+ * This allows a ground control station to automatically find the drone
+ * on the local network.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Never broadcast
+ * @value 1 Always broadcast
+ * @value 2 Only multicast
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_1_BROADCAST, 0);
 
 /**
- * Position of rotor 6 along Z body axis
+ * Broadcast heartbeats on local network for MAVLink instance 2
  *
+ * This allows a ground control station to automatically find the drone
+ * on the local network.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Never broadcast
+ * @value 1 Always broadcast
+ * @value 2 Only multicast
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_2_BROADCAST, 0);
 
 /**
- * Position of rotor 7 along Z body axis
+ * Enable serial flow control for instance 0
  *
+ * This is used to force flow control on or off for the the mavlink
+ * instance. By default it is auto detected. Use when auto detection fails.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Force off
+ * @value 1 Force on
+ * @value 2 Auto-detected
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_0_FLOW_CTRL, 2);
 
 /**
- * Position of rotor 8 along Z body axis
+ * Enable serial flow control for instance 1
  *
+ * This is used to force flow control on or off for the the mavlink
+ * instance. By default it is auto detected. Use when auto detection fails.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Force off
+ * @value 1 Force on
+ * @value 2 Auto-detected
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_1_FLOW_CTRL, 2);
 
 /**
- * Position of rotor 9 along Z body axis
+ * Enable serial flow control for instance 2
  *
+ * This is used to force flow control on or off for the the mavlink
+ * instance. By default it is auto detected. Use when auto detection fails.
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
+ * @group MAVLink
+ * @value 0 Force off
+ * @value 1 Force on
+ * @value 2 Auto-detected
+ * @reboot_required True
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_PZ, 0.0);
+PARAM_DEFINE_INT32(MAV_2_FLOW_CTRL, 2);
 
 /**
- * Position of rotor 10 along Z body axis
+ * SIM_GZ ESC 1 Output Function
  *
+ * Select what should be output on SIM_GZ ESC 1.
  * 
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_PZ, 0.0);
-
-/**
- * Position of rotor 11 along Z body axis
- *
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- * @unit m
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_PZ, 0.0);
-
-/**
- * Axis of rotor 0 thrust vector, X body axis component
- *
- * Only the direction is considered (the vector is normalized).
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_AX, 0.0);
-
-/**
- * Axis of rotor 1 thrust vector, X body axis component
- *
- * Only the direction is considered (the vector is normalized).
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC1, 0);
 
 /**
- * Axis of rotor 2 thrust vector, X body axis component
+ * SIM_GZ ESC 2 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 2.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC2, 0);
 
 /**
- * Axis of rotor 3 thrust vector, X body axis component
+ * SIM_GZ ESC 3 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 3.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC3, 0);
 
 /**
- * Axis of rotor 4 thrust vector, X body axis component
+ * SIM_GZ ESC 4 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 4.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC4, 0);
 
 /**
- * Axis of rotor 5 thrust vector, X body axis component
+ * SIM_GZ ESC 5 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 5.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC5, 0);
 
 /**
- * Axis of rotor 6 thrust vector, X body axis component
- *
- * Only the direction is considered (the vector is normalized).
+ * SIM_GZ ESC 6 Output Function
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * Select what should be output on SIM_GZ ESC 6.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
+ *
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC6, 0);
 
 /**
- * Axis of rotor 7 thrust vector, X body axis component
+ * SIM_GZ ESC 7 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 7.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC7, 0);
 
 /**
- * Axis of rotor 8 thrust vector, X body axis component
+ * SIM_GZ ESC 8 Output Function
  *
- * Only the direction is considered (the vector is normalized).
+ * Select what should be output on SIM_GZ ESC 8.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC8, 0);
 
 /**
- * Axis of rotor 9 thrust vector, X body axis component
+ * SIM_GZ ESC 1 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS1, 0);
 
 /**
- * Axis of rotor 10 thrust vector, X body axis component
+ * SIM_GZ ESC 2 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS2, 0);
 
 /**
- * Axis of rotor 11 thrust vector, X body axis component
+ * SIM_GZ ESC 3 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_AX, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS3, 0);
 
 /**
- * Axis of rotor 0 thrust vector, Y body axis component
+ * SIM_GZ ESC 4 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS4, 0);
 
 /**
- * Axis of rotor 1 thrust vector, Y body axis component
+ * SIM_GZ ESC 5 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS5, 0);
 
 /**
- * Axis of rotor 2 thrust vector, Y body axis component
+ * SIM_GZ ESC 6 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS6, 0);
 
 /**
- * Axis of rotor 3 thrust vector, Y body axis component
+ * SIM_GZ ESC 7 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS7, 0);
 
 /**
- * Axis of rotor 4 thrust vector, Y body axis component
+ * SIM_GZ ESC 8 Disarmed Value
  *
- * Only the direction is considered (the vector is normalized).
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_DIS8, 0);
 
 /**
- * Axis of rotor 5 thrust vector, Y body axis component
+ * SIM_GZ ESC 1 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN1, 0);
 
 /**
- * Axis of rotor 6 thrust vector, Y body axis component
+ * SIM_GZ ESC 2 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN2, 0);
 
 /**
- * Axis of rotor 7 thrust vector, Y body axis component
+ * SIM_GZ ESC 3 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN3, 0);
 
 /**
- * Axis of rotor 8 thrust vector, Y body axis component
+ * SIM_GZ ESC 4 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN4, 0);
 
 /**
- * Axis of rotor 9 thrust vector, Y body axis component
+ * SIM_GZ ESC 5 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN5, 0);
 
 /**
- * Axis of rotor 10 thrust vector, Y body axis component
+ * SIM_GZ ESC 6 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN6, 0);
 
 /**
- * Axis of rotor 11 thrust vector, Y body axis component
+ * SIM_GZ ESC 7 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_AY, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN7, 0);
 
 /**
- * Axis of rotor 0 thrust vector, Z body axis component
+ * SIM_GZ ESC 8 Minimum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MIN8, 0);
 
 /**
- * Axis of rotor 1 thrust vector, Z body axis component
+ * SIM_GZ ESC 1 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX1, 1000);
 
 /**
- * Axis of rotor 2 thrust vector, Z body axis component
+ * SIM_GZ ESC 2 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX2, 1000);
 
 /**
- * Axis of rotor 3 thrust vector, Z body axis component
+ * SIM_GZ ESC 3 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX3, 1000);
 
 /**
- * Axis of rotor 4 thrust vector, Z body axis component
+ * SIM_GZ ESC 4 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_AZ, -1.0);
-
-/**
- * Axis of rotor 5 thrust vector, Z body axis component
- *
- * Only the direction is considered (the vector is normalized).
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_AZ, -1.0);
-
-/**
- * Axis of rotor 6 thrust vector, Z body axis component
- *
- * Only the direction is considered (the vector is normalized).
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_AZ, -1.0);
-
-/**
- * Axis of rotor 7 thrust vector, Z body axis component
- *
- * Only the direction is considered (the vector is normalized).
- *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX4, 1000);
 
 /**
- * Axis of rotor 8 thrust vector, Z body axis component
+ * SIM_GZ ESC 5 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX5, 1000);
 
 /**
- * Axis of rotor 9 thrust vector, Z body axis component
+ * SIM_GZ ESC 6 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX6, 1000);
 
 /**
- * Axis of rotor 10 thrust vector, Z body axis component
+ * SIM_GZ ESC 7 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX7, 1000);
 
 /**
- * Axis of rotor 11 thrust vector, Z body axis component
+ * SIM_GZ ESC 8 Maximum Value
  *
- * Only the direction is considered (the vector is normalized).
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @decimal 2
- * @increment 0.1
- * @min -100
- * @max 100
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_AZ, -1.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_MAX8, 1000);
 
 /**
- * Thrust coefficient of rotor 0
+ * SIM_GZ ESC 1 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC1).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL1, -1);
 
 /**
- * Thrust coefficient of rotor 1
+ * SIM_GZ ESC 2 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC2).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL2, -1);
 
 /**
- * Thrust coefficient of rotor 2
+ * SIM_GZ ESC 3 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC3).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL3, -1);
 
 /**
- * Thrust coefficient of rotor 3
+ * SIM_GZ ESC 4 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC4).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL4, -1);
 
 /**
- * Thrust coefficient of rotor 4
+ * SIM_GZ ESC 5 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC5).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL5, -1);
 
 /**
- * Thrust coefficient of rotor 5
+ * SIM_GZ ESC 6 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC6).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL6, -1);
 
 /**
- * Thrust coefficient of rotor 6
+ * SIM_GZ ESC 7 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC7).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL7, -1);
 
 /**
- * Thrust coefficient of rotor 7
+ * SIM_GZ ESC 8 Failsafe Value
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC8).
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL8, -1);
 
 /**
- * Thrust coefficient of rotor 8
+ * SIM_GZ Servo 1 Output Function
  *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * Select what should be output on SIM_GZ Servo 1.
  * 
- *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_CT, 6.5);
-
-/**
- * Thrust coefficient of rotor 9
- *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
- * 
- *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_CT, 6.5);
-
-/**
- * Thrust coefficient of rotor 10
- *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
- * 
- *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_CT, 6.5);
-
-/**
- * Thrust coefficient of rotor 11
- *
- * The thrust coefficient if defined as Thrust = CT * u^2,
- * where u (with value between actuator minimum and maximum)
- * is the output signal sent to the motor controller.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 1
- * @increment 1
- * @min 0
- * @max 100
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_CT, 6.5);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC1, 0);
 
 /**
- * Moment coefficient of rotor 0
+ * SIM_GZ Servo 2 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 2.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR0_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC2, 0);
 
 /**
- * Moment coefficient of rotor 1
+ * SIM_GZ Servo 3 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 3.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR1_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC3, 0);
 
 /**
- * Moment coefficient of rotor 2
+ * SIM_GZ Servo 4 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 4.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
- */
-PARAM_DEFINE_FLOAT(CA_ROTOR2_KM, 0.05);
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
+ */
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC4, 0);
 
 /**
- * Moment coefficient of rotor 3
+ * SIM_GZ Servo 5 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 5.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR3_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC5, 0);
 
 /**
- * Moment coefficient of rotor 4
+ * SIM_GZ Servo 6 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 6.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR4_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC6, 0);
 
 /**
- * Moment coefficient of rotor 5
+ * SIM_GZ Servo 7 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 7.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR5_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC7, 0);
 
 /**
- * Moment coefficient of rotor 6
+ * SIM_GZ Servo 8 Output Function
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * Select what should be output on SIM_GZ Servo 8.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR6_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC8, 0);
 
 /**
- * Moment coefficient of rotor 7
+ * SIM_GZ Servo 1 Disarmed Value
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * This is the output value that is set when not armed.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR7_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS1, 500);
 
 /**
- * Moment coefficient of rotor 8
+ * SIM_GZ Servo 2 Disarmed Value
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * This is the output value that is set when not armed.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR8_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS2, 500);
 
 /**
- * Moment coefficient of rotor 9
+ * SIM_GZ Servo 3 Disarmed Value
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * This is the output value that is set when not armed.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR9_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS3, 500);
 
 /**
- * Moment coefficient of rotor 10
+ * SIM_GZ Servo 4 Disarmed Value
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * This is the output value that is set when not armed.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR10_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS4, 500);
 
 /**
- * Moment coefficient of rotor 11
+ * SIM_GZ Servo 5 Disarmed Value
  *
- * The moment coefficient if defined as Torque = KM * Thrust.
+ * This is the output value that is set when not armed.
  * 
- * Use a positive value for a rotor with CCW rotation.
- * Use a negative value for a rotor with CW rotation.
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Geometry
- * @decimal 3
- * @increment 0.01
- * @min -1
- * @max 1
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_ROTOR11_KM, 0.05);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS5, 500);
 
 /**
- * Rotor 0 tilt assignment
+ * SIM_GZ Servo 6 Disarmed Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR0_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS6, 500);
 
 /**
- * Rotor 1 tilt assignment
+ * SIM_GZ Servo 7 Disarmed Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR1_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS7, 500);
 
 /**
- * Rotor 2 tilt assignment
+ * SIM_GZ Servo 8 Disarmed Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR2_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_DIS8, 500);
 
 /**
- * Rotor 3 tilt assignment
+ * SIM_GZ Servo 1 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR3_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN1, 0);
 
 /**
- * Rotor 4 tilt assignment
+ * SIM_GZ Servo 2 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR4_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN2, 0);
 
 /**
- * Rotor 5 tilt assignment
+ * SIM_GZ Servo 3 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR5_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN3, 0);
 
 /**
- * Rotor 6 tilt assignment
+ * SIM_GZ Servo 4 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR6_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN4, 0);
 
 /**
- * Rotor 7 tilt assignment
+ * SIM_GZ Servo 5 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR7_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN5, 0);
 
 /**
- * Rotor 8 tilt assignment
+ * SIM_GZ Servo 6 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR8_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN6, 0);
 
 /**
- * Rotor 9 tilt assignment
+ * SIM_GZ Servo 7 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR9_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN7, 0);
 
 /**
- * Rotor 10 tilt assignment
+ * SIM_GZ Servo 8 Minimum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Minimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR10_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MIN8, 0);
 
 /**
- * Rotor 11 tilt assignment
+ * SIM_GZ Servo 1 Maximum Value
  *
- * If not set to None, this motor is tilted by the configured tilt servo.
+ * Maxmimum output value (when not disarmed).
+ * 
  *
- * @group Geometry
- * @value 0 None
- * @value 1 Tilt 1
- * @value 2 Tilt 2
- * @value 3 Tilt 3
- * @value 4 Tilt 4
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_ROTOR11_TILT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX1, 1000);
 
 /**
- * Total number of Control Surfaces
+ * SIM_GZ Servo 2 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 0
- * @value 1 1
- * @value 2 2
- * @value 3 3
- * @value 4 4
- * @value 5 5
- * @value 6 6
- * @value 7 7
- * @value 8 8
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS_COUNT, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX2, 1000);
 
 /**
- * Control Surface 0 type
+ * SIM_GZ Servo 3 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS0_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX3, 1000);
 
 /**
- * Control Surface 1 type
+ * SIM_GZ Servo 4 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS1_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX4, 1000);
 
 /**
- * Control Surface 2 type
+ * SIM_GZ Servo 5 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS2_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX5, 1000);
 
 /**
- * Control Surface 3 type
+ * SIM_GZ Servo 6 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS3_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX6, 1000);
 
 /**
- * Control Surface 4 type
+ * SIM_GZ Servo 7 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS4_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX7, 1000);
 
 /**
- * Control Surface 5 type
+ * SIM_GZ Servo 8 Maximum Value
  *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS5_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_MAX8, 1000);
 
 /**
- * Control Surface 6 type
+ * SIM_GZ Servo 1 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
  * 
- *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
- */
-PARAM_DEFINE_INT32(CA_SV_CS6_TYPE, 0);
-
-/**
- * Control Surface 7 type
- *
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC1).
  * 
  *
- * @group Geometry
- * @value 0 (Not set)
- * @value 1 Left Aileron
- * @value 2 Right Aileron
- * @value 3 Elevator
- * @value 4 Rudder
- * @value 5 Left Elevon
- * @value 6 Right Elevon
- * @value 7 Left V-Tail
- * @value 8 Right V-Tail
- * @value 9 Left Flap
- * @value 10 Right Flap
- * @value 11 Airbrake
- * @value 12 Custom
- * @value 13 Left A-tail
- * @value 14 Right A-tail
- * @value 15 Single Channel Aileron
- * @value 16 Steering Wheel
- * @value 17 Left Spoiler
- * @value 18 Right Spoiler
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(CA_SV_CS7_TYPE, 0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL1, -1);
 
 /**
- * Control Surface 0 roll torque scaling
+ * SIM_GZ Servo 2 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC2).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL2, -1);
 
 /**
- * Control Surface 1 roll torque scaling
+ * SIM_GZ Servo 3 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC3).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL3, -1);
 
 /**
- * Control Surface 2 roll torque scaling
+ * SIM_GZ Servo 4 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC4).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL4, -1);
 
 /**
- * Control Surface 3 roll torque scaling
+ * SIM_GZ Servo 5 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC5).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL5, -1);
 
 /**
- * Control Surface 4 roll torque scaling
+ * SIM_GZ Servo 6 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC6).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL6, -1);
 
 /**
- * Control Surface 5 roll torque scaling
+ * SIM_GZ Servo 7 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC7).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL7, -1);
 
 /**
- * Control Surface 6 roll torque scaling
+ * SIM_GZ Servo 8 Failsafe Value
  *
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC8).
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL8, -1);
 
 /**
- * Control Surface 7 roll torque scaling
+ * Reverse Output Range for SIM_GZ
  *
+ * Allows to reverse the output range for each channel.
+ * Note: this is only useful for servos.
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @bit 0 SIM_GZ ESC 1
+ * @bit 1 SIM_GZ ESC 2
+ * @bit 2 SIM_GZ ESC 3
+ * @bit 3 SIM_GZ ESC 4
+ * @bit 4 SIM_GZ ESC 5
+ * @bit 5 SIM_GZ ESC 6
+ * @bit 6 SIM_GZ ESC 7
+ * @bit 7 SIM_GZ ESC 8
+ * @min 0
+ * @max 255
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_R, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_EC_REV, 0);
 
 /**
- * Control Surface 0 pitch torque scaling
+ * Reverse Output Range for SIM_GZ
  *
+ * Allows to reverse the output range for each channel.
+ * Note: this is only useful for servos.
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group Actuator Outputs
+ * @bit 0 SIM_GZ Servo 1
+ * @bit 1 SIM_GZ Servo 2
+ * @bit 2 SIM_GZ Servo 3
+ * @bit 3 SIM_GZ Servo 4
+ * @bit 4 SIM_GZ Servo 5
+ * @bit 5 SIM_GZ Servo 6
+ * @bit 6 SIM_GZ Servo 7
+ * @bit 7 SIM_GZ Servo 8
+ * @min 0
+ * @max 255
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_P, 0.0);
+PARAM_DEFINE_INT32(SIM_GZ_SV_REV, 0);
 
 /**
- * Control Surface 1 pitch torque scaling
+ * RC input protocol
  *
+ * Select your RC input protocol or auto to scan.
  * 
  *
- * @group Geometry
- * @decimal 2
+ * @group RC Input
+ * @value -1 Auto
+ * @value 0 None
+ * @value 1 PPM
+ * @value 2 SBUS
+ * @value 3 DSM
+ * @value 4 ST24
+ * @value 5 SUMD
+ * @value 6 CRSF
+ * @value 7 GHST
+ * @category System
+ * @min -1
+ * @max 7
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_P, 0.0);
+PARAM_DEFINE_INT32(RC_INPUT_PROTO, -1);
 
 /**
- * Control Surface 2 pitch torque scaling
+ * Airframe selection
  *
+ * Defines which mixer implementation to use.
+ * Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.
+ * 
+ * 'Custom' should only be used if noting else can be used.
  * 
  *
  * @group Geometry
- * @decimal 2
+ * @value 0 Multirotor
+ * @value 1 Fixed-wing
+ * @value 2 Standard VTOL
+ * @value 3 Tiltrotor VTOL
+ * @value 4 Tailsitter VTOL
+ * @value 5 Rover (Ackermann)
+ * @value 6 Rover (Differential)
+ * @value 7 Motors (6DOF)
+ * @value 8 Multirotor with Tilt
+ * @value 9 Custom
+ * @value 10 Helicopter
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_P, 0.0);
+PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
 
 /**
- * Control Surface 3 pitch torque scaling
+ * Control allocation method
  *
+ * Selects the algorithm and desaturation method.
+ * If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
  * 
  *
  * @group Geometry
- * @decimal 2
+ * @value 0 Pseudo-inverse with output clipping
+ * @value 1 Pseudo-inverse with sequential desaturation technique
+ * @value 2 Automatic
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_P, 0.0);
+PARAM_DEFINE_INT32(CA_METHOD, 2);
 
 /**
- * Control Surface 4 pitch torque scaling
+ * Bidirectional/Reversible motors
  *
+ * Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well.
  * 
  *
  * @group Geometry
- * @decimal 2
- */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_P, 0.0);
-
-/**
- * Control Surface 5 pitch torque scaling
+ * @bit 0 Motor 1
+ * @bit 1 Motor 2
+ * @bit 2 Motor 3
+ * @bit 3 Motor 4
+ * @bit 4 Motor 5
+ * @bit 5 Motor 6
+ * @bit 6 Motor 7
+ * @bit 7 Motor 8
+ * @bit 8 Motor 9
+ * @bit 9 Motor 10
+ * @bit 10 Motor 11
+ * @bit 11 Motor 12
+ * @min 0
+ * @max 4095
+ */
+PARAM_DEFINE_INT32(CA_R_REV, 0);
+
+/**
+ * Motor 0 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_P, 0.0);
+PARAM_DEFINE_FLOAT(CA_R0_SLEW, 0.0);
 
 /**
- * Control Surface 6 pitch torque scaling
+ * Motor 1 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_P, 0.0);
+PARAM_DEFINE_FLOAT(CA_R1_SLEW, 0.0);
 
 /**
- * Control Surface 7 pitch torque scaling
+ * Motor 2 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_P, 0.0);
+PARAM_DEFINE_FLOAT(CA_R2_SLEW, 0.0);
 
 /**
- * Control Surface 0 yaw torque scaling
+ * Motor 3 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R3_SLEW, 0.0);
 
 /**
- * Control Surface 1 yaw torque scaling
+ * Motor 4 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R4_SLEW, 0.0);
 
 /**
- * Control Surface 2 yaw torque scaling
+ * Motor 5 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R5_SLEW, 0.0);
 
 /**
- * Control Surface 3 yaw torque scaling
+ * Motor 6 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R6_SLEW, 0.0);
 
 /**
- * Control Surface 4 yaw torque scaling
+ * Motor 7 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R7_SLEW, 0.0);
 
 /**
- * Control Surface 5 yaw torque scaling
+ * Motor 8 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R8_SLEW, 0.0);
 
 /**
- * Control Surface 6 yaw torque scaling
+ * Motor 9 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R9_SLEW, 0.0);
 
 /**
- * Control Surface 7 yaw torque scaling
+ * Motor 10 slew rate limit
  *
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_Y, 0.0);
+PARAM_DEFINE_FLOAT(CA_R10_SLEW, 0.0);
 
 /**
- * Control Surface 0 trim
+ * Motor 11 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the motor input signal to pass through
+ * the full output range. A value x means that the motor signal
+ * can only go from 0 to 1 in minimum x seconds (in case of
+ * reversible motors, the range is -1 to 1).
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.01
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_R11_SLEW, 0.0);
 
 /**
- * Control Surface 1 trim
+ * Servo 0 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV0_SLEW, 0.0);
 
 /**
- * Control Surface 2 trim
+ * Servo 1 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV1_SLEW, 0.0);
 
 /**
- * Control Surface 3 trim
+ * Servo 2 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV2_SLEW, 0.0);
 
 /**
- * Control Surface 4 trim
+ * Servo 3 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV3_SLEW, 0.0);
 
 /**
- * Control Surface 5 trim
+ * Servo 4 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV4_SLEW, 0.0);
 
 /**
- * Control Surface 6 trim
+ * Servo 5 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV5_SLEW, 0.0);
 
 /**
- * Control Surface 7 trim
+ * Servo 6 slew rate limit
  *
- * Can be used to add an offset to the servo control.
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
+ * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_TRIM, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV6_SLEW, 0.0);
 
 /**
- * Control Surface 0 configuration as flap
+ * Servo 7 slew rate limit
  *
+ * Minimum time allowed for the servo input signal to pass through
+ * the full output range. A value x means that the servo signal
+ * can only go from -1 to 1 in minimum x seconds.
+ * 
+ * Zero means that slew rate limiting is disabled.
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.05
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_SV7_SLEW, 0.0);
 
 /**
- * Control Surface 1 configuration as flap
+ * Total number of rotors
  *
  * 
  *
  * @group Geometry
- * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @value 0 0
+ * @value 1 1
+ * @value 2 2
+ * @value 3 3
+ * @value 4 4
+ * @value 5 5
+ * @value 6 6
+ * @value 7 7
+ * @value 8 8
+ * @value 9 9
+ * @value 10 10
+ * @value 11 11
+ * @value 12 12
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_FLAP, 0);
+PARAM_DEFINE_INT32(CA_ROTOR_COUNT, 0);
 
 /**
- * Control Surface 2 configuration as flap
+ * Position of rotor 0 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_PX, 0.0);
 
 /**
- * Control Surface 3 configuration as flap
+ * Position of rotor 1 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_PX, 0.0);
 
 /**
- * Control Surface 4 configuration as flap
+ * Position of rotor 2 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_PX, 0.0);
 
 /**
- * Control Surface 5 configuration as flap
+ * Position of rotor 3 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_PX, 0.0);
 
 /**
- * Control Surface 6 configuration as flap
+ * Position of rotor 4 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_PX, 0.0);
 
 /**
- * Control Surface 7 configuration as flap
+ * Position of rotor 5 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_FLAP, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_PX, 0.0);
 
 /**
- * Control Surface 0 configuration as spoiler
+ * Position of rotor 6 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS0_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_PX, 0.0);
 
 /**
- * Control Surface 1 configuration as spoiler
+ * Position of rotor 7 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS1_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_PX, 0.0);
 
 /**
- * Control Surface 2 configuration as spoiler
+ * Position of rotor 8 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS2_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_PX, 0.0);
 
 /**
- * Control Surface 3 configuration as spoiler
+ * Position of rotor 9 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS3_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_PX, 0.0);
 
 /**
- * Control Surface 4 configuration as spoiler
+ * Position of rotor 10 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS4_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_PX, 0.0);
 
 /**
- * Control Surface 5 configuration as spoiler
+ * Position of rotor 11 along X body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS5_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_PX, 0.0);
 
 /**
- * Control Surface 6 configuration as spoiler
+ * Position of rotor 0 along Y body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS6_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_PY, 0.0);
 
 /**
- * Control Surface 7 configuration as spoiler
+ * Position of rotor 1 along Y body axis
  *
  * 
  *
  * @group Geometry
  * @decimal 2
- * @min -1.0
- * @max 1.0
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_CS7_SPOIL, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_PY, 0.0);
 
 /**
- * Total number of Tilt Servos
+ * Position of rotor 2 along Y body axis
  *
  * 
  *
  * @group Geometry
- * @value 0 0
- * @value 1 1
- * @value 2 2
- * @value 3 3
- * @value 4 4
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL_COUNT, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_PY, 0.0);
 
 /**
- * Tilt 0 is used for control
+ * Position of rotor 3 along Y body axis
  *
- * Define if this servo is used for additional control.
+ * 
  *
  * @group Geometry
- * @value 0 None
- * @value 1 Yaw
- * @value 2 Pitch
- * @value 3 Yaw and Pitch
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL0_CT, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_PY, 0.0);
 
 /**
- * Tilt 1 is used for control
+ * Position of rotor 4 along Y body axis
  *
- * Define if this servo is used for additional control.
+ * 
  *
  * @group Geometry
- * @value 0 None
- * @value 1 Yaw
- * @value 2 Pitch
- * @value 3 Yaw and Pitch
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL1_CT, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_PY, 0.0);
 
 /**
- * Tilt 2 is used for control
+ * Position of rotor 5 along Y body axis
  *
- * Define if this servo is used for additional control.
+ * 
  *
  * @group Geometry
- * @value 0 None
- * @value 1 Yaw
- * @value 2 Pitch
- * @value 3 Yaw and Pitch
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL2_CT, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_PY, 0.0);
 
 /**
- * Tilt 3 is used for control
+ * Position of rotor 6 along Y body axis
  *
- * Define if this servo is used for additional control.
+ * 
  *
  * @group Geometry
- * @value 0 None
- * @value 1 Yaw
- * @value 2 Pitch
- * @value 3 Yaw and Pitch
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL3_CT, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_PY, 0.0);
 
 /**
- * Tilt Servo 0 Tilt Angle at Minimum
+ * Position of rotor 7 along Y body axis
  *
- * Defines the tilt angle when the servo is at the minimum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL0_MINA, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_PY, 0.0);
 
 /**
- * Tilt Servo 1 Tilt Angle at Minimum
+ * Position of rotor 8 along Y body axis
  *
- * Defines the tilt angle when the servo is at the minimum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL1_MINA, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_PY, 0.0);
 
 /**
- * Tilt Servo 2 Tilt Angle at Minimum
+ * Position of rotor 9 along Y body axis
  *
- * Defines the tilt angle when the servo is at the minimum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL2_MINA, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_PY, 0.0);
 
 /**
- * Tilt Servo 3 Tilt Angle at Minimum
+ * Position of rotor 10 along Y body axis
  *
- * Defines the tilt angle when the servo is at the minimum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL3_MINA, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_PY, 0.0);
 
 /**
- * Tilt Servo 0 Tilt Angle at Maximum
+ * Position of rotor 11 along Y body axis
  *
- * Defines the tilt angle when the servo is at the maximum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL0_MAXA, 90.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_PY, 0.0);
 
 /**
- * Tilt Servo 1 Tilt Angle at Maximum
+ * Position of rotor 0 along Z body axis
  *
- * Defines the tilt angle when the servo is at the maximum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL1_MAXA, 90.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_PZ, 0.0);
 
 /**
- * Tilt Servo 2 Tilt Angle at Maximum
+ * Position of rotor 1 along Z body axis
  *
- * Defines the tilt angle when the servo is at the maximum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL2_MAXA, 90.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_PZ, 0.0);
 
 /**
- * Tilt Servo 3 Tilt Angle at Maximum
+ * Position of rotor 2 along Z body axis
  *
- * Defines the tilt angle when the servo is at the maximum.
- * An angle of zero means upwards.
  * 
  *
  * @group Geometry
- * @decimal 0
- * @min -90.0
- * @max 90.0
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SV_TL3_MAXA, 90.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_PZ, 0.0);
 
 /**
- * Tilt Servo 0 Tilt Direction
+ * Position of rotor 3 along Z body axis
  *
- * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
- * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
- * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
  * @group Geometry
- * @value 0 Towards Front
- * @value 90 Towards Right
- * @min 0
- * @max 359
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL0_TD, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_PZ, 0.0);
 
 /**
- * Tilt Servo 1 Tilt Direction
+ * Position of rotor 4 along Z body axis
  *
- * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
- * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
- * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
  * @group Geometry
- * @value 0 Towards Front
- * @value 90 Towards Right
- * @min 0
- * @max 359
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL1_TD, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_PZ, 0.0);
 
 /**
- * Tilt Servo 2 Tilt Direction
+ * Position of rotor 5 along Z body axis
  *
- * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
- * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
- * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
  * @group Geometry
- * @value 0 Towards Front
- * @value 90 Towards Right
- * @min 0
- * @max 359
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL2_TD, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_PZ, 0.0);
 
 /**
- * Tilt Servo 3 Tilt Direction
+ * Position of rotor 6 along Z body axis
  *
- * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
- * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
- * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
  * @group Geometry
- * @value 0 Towards Front
- * @value 90 Towards Right
- * @min 0
- * @max 359
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SV_TL3_TD, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_PZ, 0.0);
 
 /**
- * Number of swash plates servos
+ * Position of rotor 7 along Z body axis
  *
  * 
  *
  * @group Geometry
- * @value 3 3
- * @value 4 4
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_INT32(CA_SP0_COUNT, 3);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_PZ, 0.0);
 
 /**
- * Angle for swash plate servo 0
+ * Position of rotor 8 along Z body axis
  *
- * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
  * @group Geometry
- * @decimal 0
- * @increment 10
- * @min 0
- * @max 360
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ANG0, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_PZ, 0.0);
 
 /**
- * Angle for swash plate servo 1
+ * Position of rotor 9 along Z body axis
  *
- * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
  * @group Geometry
- * @decimal 0
- * @increment 10
- * @min 0
- * @max 360
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ANG1, 140);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_PZ, 0.0);
 
 /**
- * Angle for swash plate servo 2
+ * Position of rotor 10 along Z body axis
  *
- * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
  * @group Geometry
- * @decimal 0
- * @increment 10
- * @min 0
- * @max 360
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ANG2, 220);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_PZ, 0.0);
 
 /**
- * Angle for swash plate servo 3
+ * Position of rotor 11 along Z body axis
  *
- * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
  * @group Geometry
- * @decimal 0
- * @increment 10
- * @min 0
- * @max 360
- * @unit deg
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
+ * @unit m
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ANG3, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_PZ, 0.0);
 
 /**
- * Arm length for swash plate servo 0
+ * Axis of rotor 0 thrust vector, X body axis component
  *
- * This is relative to the other arm lengths.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 10
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ARM_L0, 1.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_AX, 0.0);
 
 /**
- * Arm length for swash plate servo 1
+ * Axis of rotor 1 thrust vector, X body axis component
  *
- * This is relative to the other arm lengths.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 10
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ARM_L1, 1.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_AX, 0.0);
 
 /**
- * Arm length for swash plate servo 2
+ * Axis of rotor 2 thrust vector, X body axis component
  *
- * This is relative to the other arm lengths.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 10
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ARM_L2, 1.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_AX, 0.0);
 
 /**
- * Arm length for swash plate servo 3
+ * Axis of rotor 3 thrust vector, X body axis component
  *
- * This is relative to the other arm lengths.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 10
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_SP0_ARM_L3, 1.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_AX, 0.0);
 
 /**
- * Throttle curve at position 0
+ * Axis of rotor 4 thrust vector, X body axis component
  *
- * Defines the output throttle at the interval position 0.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_THR_C0, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_AX, 0.0);
 
 /**
- * Throttle curve at position 1
+ * Axis of rotor 5 thrust vector, X body axis component
  *
- * Defines the output throttle at the interval position 1.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_THR_C1, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_AX, 0.0);
 
 /**
- * Throttle curve at position 2
+ * Axis of rotor 6 thrust vector, X body axis component
  *
- * Defines the output throttle at the interval position 2.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_THR_C2, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_AX, 0.0);
 
 /**
- * Throttle curve at position 3
+ * Axis of rotor 7 thrust vector, X body axis component
  *
- * Defines the output throttle at the interval position 3.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_THR_C3, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_AX, 0.0);
 
 /**
- * Throttle curve at position 4
+ * Axis of rotor 8 thrust vector, X body axis component
  *
- * Defines the output throttle at the interval position 4.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min 0
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_THR_C4, 1);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_AX, 0.0);
 
 /**
- * Collective pitch curve at position 0
+ * Axis of rotor 9 thrust vector, X body axis component
  *
- * Defines the collective pitch at the interval position 0 for a given thrust setpoint.
- * 
- * Use negative values if the swash plate needs to move down to provide upwards thrust.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -1
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C0, -0.05);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_AX, 0.0);
 
 /**
- * Collective pitch curve at position 1
+ * Axis of rotor 10 thrust vector, X body axis component
  *
- * Defines the collective pitch at the interval position 1 for a given thrust setpoint.
- * 
- * Use negative values if the swash plate needs to move down to provide upwards thrust.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -1
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C1, 0.0725);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_AX, 0.0);
 
 /**
- * Collective pitch curve at position 2
+ * Axis of rotor 11 thrust vector, X body axis component
  *
- * Defines the collective pitch at the interval position 2 for a given thrust setpoint.
- * 
- * Use negative values if the swash plate needs to move down to provide upwards thrust.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -1
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C2, 0.2);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_AX, 0.0);
 
 /**
- * Collective pitch curve at position 3
+ * Axis of rotor 0 thrust vector, Y body axis component
  *
- * Defines the collective pitch at the interval position 3 for a given thrust setpoint.
- * 
- * Use negative values if the swash plate needs to move down to provide upwards thrust.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -1
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C3, 0.325);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_AY, 0.0);
 
 /**
- * Collective pitch curve at position 4
+ * Axis of rotor 1 thrust vector, Y body axis component
  *
- * Defines the collective pitch at the interval position 4 for a given thrust setpoint.
- * 
- * Use negative values if the swash plate needs to move down to provide upwards thrust.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -1
- * @max 1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C4, 0.45);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_AY, 0.0);
 
 /**
- * Scale for yaw compensation based on collective pitch
+ * Axis of rotor 2 thrust vector, Y body axis component
  *
- * This allows to add a proportional factor of the collective pitch command to the yaw command.
- * A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
- * 
- * tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -2
- * @max 2
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_YAW_CP_S, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_AY, 0.0);
 
 /**
- * Offset for yaw compensation based on collective pitch
+ * Axis of rotor 3 thrust vector, Y body axis component
  *
- * This allows to specify which collective pitch command results in the least amount of rotor drag.
- * This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch
- * by aligning the lowest rotor drag with zero compensation.
- * For symmetric profile blades this is the command that results in exactly 0° collective blade angle.
- * For lift profile blades this is typically a command resulting in slightly negative collective blade angle.
- * 
- * tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -2
- * @max 2
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_YAW_CP_O, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_AY, 0.0);
 
 /**
- * Scale for yaw compensation based on throttle
+ * Axis of rotor 4 thrust vector, Y body axis component
  *
- * This allows to add a proportional factor of the throttle command to the yaw command.
- * A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
- * 
- * tail_output += CA_HELI_YAW_TH_S * throttle
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @decimal 3
+ * @decimal 2
  * @increment 0.1
- * @min -2
- * @max 2
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_FLOAT(CA_HELI_YAW_TH_S, 0.0);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_AY, 0.0);
 
 /**
- * Main rotor turns counter-clockwise
+ * Axis of rotor 5 thrust vector, Y body axis component
  *
- * Default configuration is for a clockwise turning main rotor and
- * positive thrust of the tail rotor is expected to rotate the vehicle clockwise.
- * Set this parameter to true if the tail rotor provides thrust in counter-clockwise direction
- * which is mostly the case when the main rotor turns counter-clockwise.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @boolean
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(CA_HELI_YAW_CCW, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_AY, 0.0);
 
 /**
- * Motor failure handling mode
+ * Axis of rotor 6 thrust vector, Y body axis component
  *
- * This is used to specify how to handle motor failures
- * reported by failure detector.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
  * @group Geometry
- * @value 0 Ignore
- * @value 1 Remove first failed motor from effectiveness
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(CA_FAILURE_MODE, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_AY, 0.0);
 
 /**
- * SIM_GZ ESC 1 Output Function
+ * Axis of rotor 7 thrust vector, Y body axis component
  *
- * Select what should be output on SIM_GZ ESC 1.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC1, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_AY, 0.0);
 
 /**
- * SIM_GZ ESC 2 Output Function
+ * Axis of rotor 8 thrust vector, Y body axis component
  *
- * Select what should be output on SIM_GZ ESC 2.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC2, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_AY, 0.0);
 
 /**
- * SIM_GZ ESC 3 Output Function
+ * Axis of rotor 9 thrust vector, Y body axis component
  *
- * Select what should be output on SIM_GZ ESC 3.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC3, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_AY, 0.0);
 
 /**
- * SIM_GZ ESC 4 Output Function
+ * Axis of rotor 10 thrust vector, Y body axis component
  *
- * Select what should be output on SIM_GZ ESC 4.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC4, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_AY, 0.0);
 
 /**
- * SIM_GZ ESC 5 Output Function
+ * Axis of rotor 11 thrust vector, Y body axis component
  *
- * Select what should be output on SIM_GZ ESC 5.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC5, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_AY, 0.0);
 
 /**
- * SIM_GZ ESC 6 Output Function
+ * Axis of rotor 0 thrust vector, Z body axis component
  *
- * Select what should be output on SIM_GZ ESC 6.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC6, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 7 Output Function
+ * Axis of rotor 1 thrust vector, Z body axis component
  *
- * Select what should be output on SIM_GZ ESC 7.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC7, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 8 Output Function
+ * Axis of rotor 2 thrust vector, Z body axis component
  *
- * Select what should be output on SIM_GZ ESC 8.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FUNC8, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 1 Disarmed Value
+ * Axis of rotor 3 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS1, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 2 Disarmed Value
+ * Axis of rotor 4 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS2, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 3 Disarmed Value
+ * Axis of rotor 5 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS3, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 4 Disarmed Value
+ * Axis of rotor 6 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS4, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 5 Disarmed Value
+ * Axis of rotor 7 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS5, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 6 Disarmed Value
+ * Axis of rotor 8 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS6, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 7 Disarmed Value
+ * Axis of rotor 9 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS7, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 8 Disarmed Value
+ * Axis of rotor 10 thrust vector, Z body axis component
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_DIS8, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 1 Minimum Value
+ * Axis of rotor 11 thrust vector, Z body axis component
  *
- * Minimum output value (when not disarmed).
- * 
+ * Only the direction is considered (the vector is normalized).
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @increment 0.1
+ * @min -100
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN1, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_AZ, -1.0);
 
 /**
- * SIM_GZ ESC 2 Minimum Value
+ * Thrust coefficient of rotor 0
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN2, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_CT, 6.5);
 
 /**
- * SIM_GZ ESC 3 Minimum Value
+ * Thrust coefficient of rotor 1
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN3, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_CT, 6.5);
 
 /**
- * SIM_GZ ESC 4 Minimum Value
+ * Thrust coefficient of rotor 2
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN4, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_CT, 6.5);
 
 /**
- * SIM_GZ ESC 5 Minimum Value
+ * Thrust coefficient of rotor 3
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN5, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_CT, 6.5);
 
 /**
- * SIM_GZ ESC 6 Minimum Value
+ * Thrust coefficient of rotor 4
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN6, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_CT, 6.5);
 
 /**
- * SIM_GZ ESC 7 Minimum Value
+ * Thrust coefficient of rotor 5
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN7, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_CT, 6.5);
 
 /**
- * SIM_GZ ESC 8 Minimum Value
+ * Thrust coefficient of rotor 6
  *
- * Minimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MIN8, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_CT, 6.5);
 
 /**
- * SIM_GZ ESC 1 Maximum Value
+ * Thrust coefficient of rotor 7
  *
- * Maxmimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX1, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_CT, 6.5);
 
 /**
- * SIM_GZ ESC 2 Maximum Value
+ * Thrust coefficient of rotor 8
  *
- * Maxmimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX2, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_CT, 6.5);
 
 /**
- * SIM_GZ ESC 3 Maximum Value
+ * Thrust coefficient of rotor 9
  *
- * Maxmimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX3, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_CT, 6.5);
 
 /**
- * SIM_GZ ESC 4 Maximum Value
+ * Thrust coefficient of rotor 10
  *
- * Maxmimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX4, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_CT, 6.5);
 
 /**
- * SIM_GZ ESC 5 Maximum Value
+ * Thrust coefficient of rotor 11
  *
- * Maxmimum output value (when not disarmed).
+ * The thrust coefficient if defined as Thrust = CT * u^2,
+ * where u (with value between actuator minimum and maximum)
+ * is the output signal sent to the motor controller.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 1
+ * @increment 1
  * @min 0
- * @max 1000
+ * @max 100
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX5, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_CT, 6.5);
 
 /**
- * SIM_GZ ESC 6 Maximum Value
+ * Moment coefficient of rotor 0
  *
- * Maxmimum output value (when not disarmed).
+ * The moment coefficient if defined as Torque = KM * Thrust.
+ * 
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX6, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR0_KM, 0.05);
 
 /**
- * SIM_GZ ESC 7 Maximum Value
+ * Moment coefficient of rotor 1
  *
- * Maxmimum output value (when not disarmed).
+ * The moment coefficient if defined as Torque = KM * Thrust.
+ * 
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX7, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR1_KM, 0.05);
 
 /**
- * SIM_GZ ESC 8 Maximum Value
+ * Moment coefficient of rotor 2
  *
- * Maxmimum output value (when not disarmed).
+ * The moment coefficient if defined as Torque = KM * Thrust.
+ * 
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_MAX8, 1000);
+PARAM_DEFINE_FLOAT(CA_ROTOR2_KM, 0.05);
 
 /**
- * SIM_GZ ESC 1 Failsafe Value
+ * Moment coefficient of rotor 3
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC1).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL1, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR3_KM, 0.05);
 
 /**
- * SIM_GZ ESC 2 Failsafe Value
+ * Moment coefficient of rotor 4
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC2).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL2, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR4_KM, 0.05);
 
 /**
- * SIM_GZ ESC 3 Failsafe Value
+ * Moment coefficient of rotor 5
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC3).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL3, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR5_KM, 0.05);
 
 /**
- * SIM_GZ ESC 4 Failsafe Value
+ * Moment coefficient of rotor 6
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC4).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL4, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR6_KM, 0.05);
 
 /**
- * SIM_GZ ESC 5 Failsafe Value
+ * Moment coefficient of rotor 7
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC5).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL5, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR7_KM, 0.05);
 
 /**
- * SIM_GZ ESC 6 Failsafe Value
+ * Moment coefficient of rotor 8
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC6).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL6, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR8_KM, 0.05);
 
 /**
- * SIM_GZ ESC 7 Failsafe Value
+ * Moment coefficient of rotor 9
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC7).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL7, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR9_KM, 0.05);
 
 /**
- * SIM_GZ ESC 8 Failsafe Value
+ * Moment coefficient of rotor 10
  *
- * This is the output value that is set when in failsafe mode.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_EC_FUNC8).
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
  * @min -1
- * @max 1000
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_FAIL8, -1);
+PARAM_DEFINE_FLOAT(CA_ROTOR10_KM, 0.05);
 
 /**
- * SIM_GZ Servo 1 Output Function
+ * Moment coefficient of rotor 11
  *
- * Select what should be output on SIM_GZ Servo 1.
+ * The moment coefficient if defined as Torque = KM * Thrust.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ * Use a positive value for a rotor with CCW rotation.
+ * Use a negative value for a rotor with CW rotation.
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.01
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC1, 0);
+PARAM_DEFINE_FLOAT(CA_ROTOR11_KM, 0.05);
 
 /**
- * SIM_GZ Servo 2 Output Function
+ * Rotor 0 tilt assignment
  *
- * Select what should be output on SIM_GZ Servo 2.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * If not set to None, this motor is tilted by the configured tilt servo.
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC2, 0);
+PARAM_DEFINE_INT32(CA_ROTOR0_TILT, 0);
 
 /**
- * SIM_GZ Servo 3 Output Function
+ * Rotor 1 tilt assignment
  *
- * Select what should be output on SIM_GZ Servo 3.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * If not set to None, this motor is tilted by the configured tilt servo.
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC3, 0);
+PARAM_DEFINE_INT32(CA_ROTOR1_TILT, 0);
 
 /**
- * SIM_GZ Servo 4 Output Function
+ * Rotor 2 tilt assignment
  *
- * Select what should be output on SIM_GZ Servo 4.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * If not set to None, this motor is tilted by the configured tilt servo.
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC4, 0);
+PARAM_DEFINE_INT32(CA_ROTOR2_TILT, 0);
 
 /**
- * SIM_GZ Servo 5 Output Function
+ * Rotor 3 tilt assignment
  *
- * Select what should be output on SIM_GZ Servo 5.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * If not set to None, this motor is tilted by the configured tilt servo.
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC5, 0);
+PARAM_DEFINE_INT32(CA_ROTOR3_TILT, 0);
 
 /**
- * SIM_GZ Servo 6 Output Function
+ * Rotor 4 tilt assignment
  *
- * Select what should be output on SIM_GZ Servo 6.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
+ * If not set to None, this motor is tilted by the configured tilt servo.
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR4_TILT, 0);
+
+/**
+ * Rotor 5 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR5_TILT, 0);
+
+/**
+ * Rotor 6 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR6_TILT, 0);
+
+/**
+ * Rotor 7 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR7_TILT, 0);
+
+/**
+ * Rotor 8 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR8_TILT, 0);
+
+/**
+ * Rotor 9 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR9_TILT, 0);
+
+/**
+ * Rotor 10 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR10_TILT, 0);
+
+/**
+ * Rotor 11 tilt assignment
+ *
+ * If not set to None, this motor is tilted by the configured tilt servo.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Tilt 1
+ * @value 2 Tilt 2
+ * @value 3 Tilt 3
+ * @value 4 Tilt 4
+ */
+PARAM_DEFINE_INT32(CA_ROTOR11_TILT, 0);
+
+/**
+ * Total number of Control Surfaces
+ *
+ * 
+ *
+ * @group Geometry
+ * @value 0 0
+ * @value 1 1
+ * @value 2 2
+ * @value 3 3
+ * @value 4 4
+ * @value 5 5
+ * @value 6 6
+ * @value 7 7
+ * @value 8 8
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC6, 0);
+PARAM_DEFINE_INT32(CA_SV_CS_COUNT, 0);
 
 /**
- * SIM_GZ Servo 7 Output Function
+ * Control Surface 0 type
  *
- * Select what should be output on SIM_GZ Servo 7.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC7, 0);
+PARAM_DEFINE_INT32(CA_SV_CS0_TYPE, 0);
 
 /**
- * SIM_GZ Servo 8 Output Function
+ * Control Surface 1 type
  *
- * Select what should be output on SIM_GZ Servo 8.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ *
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
+ */
+PARAM_DEFINE_INT32(CA_SV_CS1_TYPE, 0);
+
+/**
+ * Control Surface 2 type
+ *
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FUNC8, 0);
+PARAM_DEFINE_INT32(CA_SV_CS2_TYPE, 0);
 
 /**
- * SIM_GZ Servo 1 Disarmed Value
+ * Control Surface 3 type
  *
- * This is the output value that is set when not armed.
  * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
+ *
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
+ */
+PARAM_DEFINE_INT32(CA_SV_CS3_TYPE, 0);
+
+/**
+ * Control Surface 4 type
+ *
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS1, 500);
+PARAM_DEFINE_INT32(CA_SV_CS4_TYPE, 0);
 
 /**
- * SIM_GZ Servo 2 Disarmed Value
+ * Control Surface 5 type
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS2, 500);
+PARAM_DEFINE_INT32(CA_SV_CS5_TYPE, 0);
 
 /**
- * SIM_GZ Servo 3 Disarmed Value
+ * Control Surface 6 type
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS3, 500);
+PARAM_DEFINE_INT32(CA_SV_CS6_TYPE, 0);
 
 /**
- * SIM_GZ Servo 4 Disarmed Value
+ * Control Surface 7 type
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @value 0 (Not set)
+ * @value 1 Left Aileron
+ * @value 2 Right Aileron
+ * @value 3 Elevator
+ * @value 4 Rudder
+ * @value 5 Left Elevon
+ * @value 6 Right Elevon
+ * @value 7 Left V-Tail
+ * @value 8 Right V-Tail
+ * @value 9 Left Flap
+ * @value 10 Right Flap
+ * @value 11 Airbrake
+ * @value 12 Custom
+ * @value 13 Left A-tail
+ * @value 14 Right A-tail
+ * @value 15 Single Channel Aileron
+ * @value 16 Steering Wheel
+ * @value 17 Left Spoiler
+ * @value 18 Right Spoiler
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS4, 500);
+PARAM_DEFINE_INT32(CA_SV_CS7_TYPE, 0);
 
 /**
- * SIM_GZ Servo 5 Disarmed Value
+ * Control Surface 0 roll torque scaling
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS5, 500);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 6 Disarmed Value
+ * Control Surface 1 roll torque scaling
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS6, 500);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 7 Disarmed Value
+ * Control Surface 2 roll torque scaling
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS7, 500);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 8 Disarmed Value
+ * Control Surface 3 roll torque scaling
  *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_DIS8, 500);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 1 Minimum Value
+ * Control Surface 4 roll torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN1, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 2 Minimum Value
+ * Control Surface 5 roll torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN2, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 3 Minimum Value
+ * Control Surface 6 roll torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN3, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 4 Minimum Value
+ * Control Surface 7 roll torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN4, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_R, 0.0);
 
 /**
- * SIM_GZ Servo 5 Minimum Value
+ * Control Surface 0 pitch torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN5, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 6 Minimum Value
+ * Control Surface 1 pitch torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN6, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 7 Minimum Value
+ * Control Surface 2 pitch torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN7, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 8 Minimum Value
+ * Control Surface 3 pitch torque scaling
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MIN8, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 1 Maximum Value
+ * Control Surface 4 pitch torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX1, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 2 Maximum Value
+ * Control Surface 5 pitch torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX2, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 3 Maximum Value
+ * Control Surface 6 pitch torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX3, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 4 Maximum Value
+ * Control Surface 7 pitch torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ *
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX4, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_P, 0.0);
 
 /**
- * SIM_GZ Servo 5 Maximum Value
+ * Control Surface 0 yaw torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX5, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 6 Maximum Value
+ * Control Surface 1 yaw torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX6, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 7 Maximum Value
+ * Control Surface 2 yaw torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX7, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 8 Maximum Value
+ * Control Surface 3 yaw torque scaling
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_MAX8, 1000);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 1 Failsafe Value
+ * Control Surface 4 yaw torque scaling
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC1).
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL1, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 2 Failsafe Value
+ * Control Surface 5 yaw torque scaling
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC2).
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL2, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 3 Failsafe Value
+ * Control Surface 6 yaw torque scaling
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC3).
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL3, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 4 Failsafe Value
+ * Control Surface 7 yaw torque scaling
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC4).
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL4, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_TRQ_Y, 0.0);
 
 /**
- * SIM_GZ Servo 5 Failsafe Value
+ * Control Surface 0 trim
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC5).
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL5, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_TRIM, 0.0);
 
 /**
- * SIM_GZ Servo 6 Failsafe Value
+ * Control Surface 1 trim
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC6).
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL6, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_TRIM, 0.0);
 
 /**
- * SIM_GZ Servo 7 Failsafe Value
+ * Control Surface 2 trim
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC7).
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL7, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_TRIM, 0.0);
 
 /**
- * SIM_GZ Servo 8 Failsafe Value
+ * Control Surface 3 trim
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see SIM_GZ_SV_FUNC8).
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_FAIL8, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_TRIM, 0.0);
 
 /**
- * Reverse Output Range for SIM_GZ
+ * Control Surface 4 trim
  *
- * Allows to reverse the output range for each channel.
- * Note: this is only useful for servos.
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @bit 0 SIM_GZ ESC 1
- * @bit 1 SIM_GZ ESC 2
- * @bit 2 SIM_GZ ESC 3
- * @bit 3 SIM_GZ ESC 4
- * @bit 4 SIM_GZ ESC 5
- * @bit 5 SIM_GZ ESC 6
- * @bit 6 SIM_GZ ESC 7
- * @bit 7 SIM_GZ ESC 8
- * @min 0
- * @max 255
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_EC_REV, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_TRIM, 0.0);
 
 /**
- * Reverse Output Range for SIM_GZ
+ * Control Surface 5 trim
  *
- * Allows to reverse the output range for each channel.
- * Note: this is only useful for servos.
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Actuator Outputs
- * @bit 0 SIM_GZ Servo 1
- * @bit 1 SIM_GZ Servo 2
- * @bit 2 SIM_GZ Servo 3
- * @bit 3 SIM_GZ Servo 4
- * @bit 4 SIM_GZ Servo 5
- * @bit 5 SIM_GZ Servo 6
- * @bit 6 SIM_GZ Servo 7
- * @bit 7 SIM_GZ Servo 8
- * @min 0
- * @max 255
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(SIM_GZ_SV_REV, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_TRIM, 0.0);
 
 /**
- * Battery 1 voltage divider (V divider)
+ * Control Surface 6 trim
  *
- * This is the divider from battery 1 voltage to ADC voltage.
- * If using e.g. Mauch power modules the value from the datasheet
- * can be applied straight here. A value of -1 means to use
- * the board default.
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Battery Calibration
- * @decimal 8
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_FLOAT(BAT1_V_DIV, -1.0);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_TRIM, 0.0);
 
 /**
- * Battery 2 voltage divider (V divider)
+ * Control Surface 7 trim
  *
- * This is the divider from battery 2 voltage to ADC voltage.
- * If using e.g. Mauch power modules the value from the datasheet
- * can be applied straight here. A value of -1 means to use
- * the board default.
- * 
+ * Can be used to add an offset to the servo control.
  *
- * @group Battery Calibration
- * @decimal 8
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_FLOAT(BAT2_V_DIV, -1.0);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_TRIM, 0.0);
 
 /**
- * Battery 1 current per volt (A/V)
+ * Control Surface 0 configuration as flap
  *
- * The voltage seen by the ADC multiplied by this factor
- * will determine the battery current. A value of -1 means to use
- * the board default.
  * 
  *
- * @group Battery Calibration
- * @decimal 8
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_FLOAT(BAT1_A_PER_V, -1.0);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_FLAP, 0);
 
 /**
- * Battery 2 current per volt (A/V)
+ * Control Surface 1 configuration as flap
  *
- * The voltage seen by the ADC multiplied by this factor
- * will determine the battery current. A value of -1 means to use
- * the board default.
  * 
  *
- * @group Battery Calibration
- * @decimal 8
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_FLOAT(BAT2_A_PER_V, -1.0);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_FLAP, 0);
 
 /**
- * Battery 1 Voltage ADC Channel
+ * Control Surface 2 configuration as flap
  *
- * This parameter specifies the ADC channel used to monitor voltage of main power battery.
- * A value of -1 means to use the board default.
  * 
  *
- * @group Battery Calibration
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(BAT1_V_CHANNEL, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_FLAP, 0);
 
 /**
- * Battery 2 Voltage ADC Channel
+ * Control Surface 3 configuration as flap
  *
- * This parameter specifies the ADC channel used to monitor voltage of main power battery.
- * A value of -1 means to use the board default.
  * 
  *
- * @group Battery Calibration
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(BAT2_V_CHANNEL, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_FLAP, 0);
 
 /**
- * Battery 1 Current ADC Channel
+ * Control Surface 4 configuration as flap
  *
- * This parameter specifies the ADC channel used to monitor current of main power battery.
- * A value of -1 means to use the board default.
  * 
  *
- * @group Battery Calibration
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(BAT1_I_CHANNEL, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_FLAP, 0);
 
 /**
- * Battery 2 Current ADC Channel
+ * Control Surface 5 configuration as flap
  *
- * This parameter specifies the ADC channel used to monitor current of main power battery.
- * A value of -1 means to use the board default.
  * 
  *
- * @group Battery Calibration
- * @reboot_required True
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(BAT2_I_CHANNEL, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_FLAP, 0);
 
 /**
- * RC input protocol
+ * Control Surface 6 configuration as flap
  *
- * Select your RC input protocol or auto to scan.
  * 
  *
- * @group RC Input
- * @value -1 Auto
- * @value 0 None
- * @value 1 PPM
- * @value 2 SBUS
- * @value 3 DSM
- * @value 4 ST24
- * @value 5 SUMD
- * @value 6 CRSF
- * @value 7 GHST
- * @category System
- * @min -1
- * @max 7
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(RC_INPUT_PROTO, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_FLAP, 0);
 
 /**
- * Accelerometer 0 calibration device ID
+ * Control Surface 7 configuration as flap
  *
- * Device ID of the accelerometer this calibration applies to.
+ * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_FLAP, 0);
 
 /**
- * Accelerometer 1 calibration device ID
+ * Control Surface 0 configuration as spoiler
  *
- * Device ID of the accelerometer this calibration applies to.
+ * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS0_SPOIL, 0);
 
 /**
- * Accelerometer 2 calibration device ID
+ * Control Surface 1 configuration as spoiler
  *
- * Device ID of the accelerometer this calibration applies to.
+ * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS1_SPOIL, 0);
 
 /**
- * Accelerometer 3 calibration device ID
+ * Control Surface 2 configuration as spoiler
  *
- * Device ID of the accelerometer this calibration applies to.
+ * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC3_ID, 0);
+PARAM_DEFINE_FLOAT(CA_SV_CS2_SPOIL, 0);
 
 /**
- * Accelerometer 0 priority
+ * Control Surface 3 configuration as spoiler
  *
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS3_SPOIL, 0);
 
 /**
- * Accelerometer 1 priority
+ * Control Surface 4 configuration as spoiler
  *
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS4_SPOIL, 0);
 
 /**
- * Accelerometer 2 priority
+ * Control Surface 5 configuration as spoiler
  *
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS5_SPOIL, 0);
 
 /**
- * Accelerometer 3 priority
+ * Control Surface 6 configuration as spoiler
  *
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @decimal 3
- * @category System
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC3_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS6_SPOIL, 0);
 
 /**
- * Accelerometer 0 rotation relative to airframe
+ * Control Surface 7 configuration as spoiler
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
+ * @group Geometry
+ * @decimal 2
+ * @min -1.0
+ * @max 1.0
  */
-PARAM_DEFINE_INT32(CAL_ACC0_ROT, -1);
+PARAM_DEFINE_FLOAT(CA_SV_CS7_SPOIL, 0);
 
 /**
- * Accelerometer 1 rotation relative to airframe
+ * Total number of Tilt Servos
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
+ * @group Geometry
+ * @value 0 0
+ * @value 1 1
+ * @value 2 2
+ * @value 3 3
+ * @value 4 4
  */
-PARAM_DEFINE_INT32(CAL_ACC1_ROT, -1);
+PARAM_DEFINE_INT32(CA_SV_TL_COUNT, 0);
 
 /**
- * Accelerometer 2 rotation relative to airframe
+ * Tilt 0 is used for control
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
- * 
+ * Define if this servo is used for additional control.
  *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Yaw
+ * @value 2 Pitch
+ * @value 3 Yaw and Pitch
  */
-PARAM_DEFINE_INT32(CAL_ACC2_ROT, -1);
+PARAM_DEFINE_INT32(CA_SV_TL0_CT, 1);
 
 /**
- * Accelerometer 3 rotation relative to airframe
+ * Tilt 1 is used for control
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
- * 
+ * Define if this servo is used for additional control.
  *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Yaw
+ * @value 2 Pitch
+ * @value 3 Yaw and Pitch
  */
-PARAM_DEFINE_INT32(CAL_ACC3_ROT, -1);
+PARAM_DEFINE_INT32(CA_SV_TL1_CT, 1);
 
 /**
- * Accelerometer 0 X-axis offset
+ * Tilt 2 is used for control
  *
- * 
+ * Define if this servo is used for additional control.
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Yaw
+ * @value 2 Pitch
+ * @value 3 Yaw and Pitch
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0);
+PARAM_DEFINE_INT32(CA_SV_TL2_CT, 1);
 
 /**
- * Accelerometer 1 X-axis offset
+ * Tilt 3 is used for control
+ *
+ * Define if this servo is used for additional control.
+ *
+ * @group Geometry
+ * @value 0 None
+ * @value 1 Yaw
+ * @value 2 Pitch
+ * @value 3 Yaw and Pitch
+ */
+PARAM_DEFINE_INT32(CA_SV_TL3_CT, 1);
+
+/**
+ * Tilt Servo 0 Tilt Angle at Minimum
  *
+ * Defines the tilt angle when the servo is at the minimum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL0_MINA, 0.0);
 
 /**
- * Accelerometer 2 X-axis offset
+ * Tilt Servo 1 Tilt Angle at Minimum
  *
+ * Defines the tilt angle when the servo is at the minimum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL1_MINA, 0.0);
 
 /**
- * Accelerometer 3 X-axis offset
+ * Tilt Servo 2 Tilt Angle at Minimum
  *
+ * Defines the tilt angle when the servo is at the minimum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
+ */
+PARAM_DEFINE_FLOAT(CA_SV_TL2_MINA, 0.0);
+
+/**
+ * Tilt Servo 3 Tilt Angle at Minimum
+ *
+ * Defines the tilt angle when the servo is at the minimum.
+ * An angle of zero means upwards.
+ * 
+ *
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL3_MINA, 0.0);
 
 /**
- * Accelerometer 0 Y-axis offset
+ * Tilt Servo 0 Tilt Angle at Maximum
  *
+ * Defines the tilt angle when the servo is at the maximum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL0_MAXA, 90.0);
 
 /**
- * Accelerometer 1 Y-axis offset
+ * Tilt Servo 1 Tilt Angle at Maximum
  *
+ * Defines the tilt angle when the servo is at the maximum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL1_MAXA, 90.0);
 
 /**
- * Accelerometer 2 Y-axis offset
+ * Tilt Servo 2 Tilt Angle at Maximum
  *
+ * Defines the tilt angle when the servo is at the maximum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL2_MAXA, 90.0);
 
 /**
- * Accelerometer 3 Y-axis offset
+ * Tilt Servo 3 Tilt Angle at Maximum
  *
+ * Defines the tilt angle when the servo is at the maximum.
+ * An angle of zero means upwards.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @decimal 0
+ * @min -90.0
+ * @max 90.0
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_SV_TL3_MAXA, 90.0);
 
 /**
- * Accelerometer 0 Z-axis offset
+ * Tilt Servo 0 Tilt Direction
  *
+ * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
+ * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
+ * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @value 0 Towards Front
+ * @value 90 Towards Right
+ * @min 0
+ * @max 359
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CA_SV_TL0_TD, 0);
 
 /**
- * Accelerometer 1 Z-axis offset
+ * Tilt Servo 1 Tilt Direction
  *
+ * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
+ * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
+ * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @value 0 Towards Front
+ * @value 90 Towards Right
+ * @min 0
+ * @max 359
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CA_SV_TL1_TD, 0);
 
 /**
- * Accelerometer 2 Z-axis offset
+ * Tilt Servo 2 Tilt Direction
  *
+ * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
+ * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
+ * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @value 0 Towards Front
+ * @value 90 Towards Right
+ * @min 0
+ * @max 359
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CA_SV_TL2_TD, 0);
 
 /**
- * Accelerometer 3 Z-axis offset
+ * Tilt Servo 3 Tilt Direction
  *
+ * Defines the direction the servo tilts towards when moving towards the maximum tilt angle.
+ * For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front',
+ * the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @unit m/s^2
+ * @group Geometry
+ * @value 0 Towards Front
+ * @value 90 Towards Right
+ * @min 0
+ * @max 359
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CA_SV_TL3_TD, 0);
 
 /**
- * Accelerometer 0 X-axis scaling factor
+ * Number of swash plates servos
  *
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @group Geometry
+ * @value 3 3
+ * @value 4 4
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0);
+PARAM_DEFINE_INT32(CA_SP0_COUNT, 3);
 
 /**
- * Accelerometer 1 X-axis scaling factor
+ * Angle for swash plate servo 0
  *
+ * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @group Geometry
+ * @decimal 0
+ * @increment 10
+ * @min 0
+ * @max 360
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ANG0, 0);
 
 /**
- * Accelerometer 2 X-axis scaling factor
+ * Angle for swash plate servo 1
  *
+ * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @group Geometry
+ * @decimal 0
+ * @increment 10
+ * @min 0
+ * @max 360
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ANG1, 140);
 
 /**
- * Accelerometer 3 X-axis scaling factor
+ * Angle for swash plate servo 2
  *
+ * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @group Geometry
+ * @decimal 0
+ * @increment 10
+ * @min 0
+ * @max 360
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_XSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ANG2, 220);
 
 /**
- * Accelerometer 0 Y-axis scaling factor
+ * Angle for swash plate servo 3
  *
+ * The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @group Geometry
+ * @decimal 0
+ * @increment 10
+ * @min 0
+ * @max 360
+ * @unit deg
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ANG3, 0);
 
 /**
- * Accelerometer 1 Y-axis scaling factor
+ * Arm length for swash plate servo 0
  *
+ * This is relative to the other arm lengths.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ARM_L0, 1.0);
 
 /**
- * Accelerometer 2 Y-axis scaling factor
+ * Arm length for swash plate servo 1
  *
+ * This is relative to the other arm lengths.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ARM_L1, 1.0);
 
 /**
- * Accelerometer 3 Y-axis scaling factor
+ * Arm length for swash plate servo 2
  *
+ * This is relative to the other arm lengths.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ARM_L2, 1.0);
 
 /**
- * Accelerometer 0 Z-axis scaling factor
+ * Arm length for swash plate servo 3
  *
+ * This is relative to the other arm lengths.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 10
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_SP0_ARM_L3, 1.0);
 
 /**
- * Accelerometer 1 Z-axis scaling factor
+ * Throttle curve at position 0
  *
+ * Defines the output throttle at the interval position 0.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 1
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_HELI_THR_C0, 1);
 
 /**
- * Accelerometer 2 Z-axis scaling factor
+ * Throttle curve at position 1
  *
+ * Defines the output throttle at the interval position 1.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 1
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_HELI_THR_C1, 1);
 
 /**
- * Accelerometer 3 Z-axis scaling factor
+ * Throttle curve at position 2
  *
+ * Defines the output throttle at the interval position 2.
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @increment 0.1
+ * @min 0
+ * @max 1
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CA_HELI_THR_C2, 1);
 
 /**
- * Barometer 0 calibration device ID
+ * Throttle curve at position 3
  *
- * Device ID of the barometer this calibration applies to.
+ * Defines the output throttle at the interval position 3.
+ * 
  *
- * @group Sensor Calibration
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min 0
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO0_ID, 0);
+PARAM_DEFINE_FLOAT(CA_HELI_THR_C3, 1);
 
 /**
- * Barometer 1 calibration device ID
+ * Throttle curve at position 4
  *
- * Device ID of the barometer this calibration applies to.
+ * Defines the output throttle at the interval position 4.
+ * 
  *
- * @group Sensor Calibration
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min 0
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO1_ID, 0);
+PARAM_DEFINE_FLOAT(CA_HELI_THR_C4, 1);
 
 /**
- * Barometer 2 calibration device ID
+ * Collective pitch curve at position 0
  *
- * Device ID of the barometer this calibration applies to.
+ * Defines the collective pitch at the interval position 0 for a given thrust setpoint.
+ * 
+ * Use negative values if the swash plate needs to move down to provide upwards thrust.
+ * 
  *
- * @group Sensor Calibration
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO2_ID, 0);
+PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C0, -0.05);
 
 /**
- * Barometer 3 calibration device ID
+ * Collective pitch curve at position 1
  *
- * Device ID of the barometer this calibration applies to.
+ * Defines the collective pitch at the interval position 1 for a given thrust setpoint.
+ * 
+ * Use negative values if the swash plate needs to move down to provide upwards thrust.
+ * 
  *
- * @group Sensor Calibration
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO3_ID, 0);
+PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C1, 0.0725);
 
 /**
- * Barometer 0 priority
+ * Collective pitch curve at position 2
  *
+ * Defines the collective pitch at the interval position 2 for a given thrust setpoint.
+ * 
+ * Use negative values if the swash plate needs to move down to provide upwards thrust.
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO0_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C2, 0.2);
 
 /**
- * Barometer 1 priority
+ * Collective pitch curve at position 3
  *
+ * Defines the collective pitch at the interval position 3 for a given thrust setpoint.
+ * 
+ * Use negative values if the swash plate needs to move down to provide upwards thrust.
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO1_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C3, 0.325);
 
 /**
- * Barometer 2 priority
+ * Collective pitch curve at position 4
  *
+ * Defines the collective pitch at the interval position 4 for a given thrust setpoint.
+ * 
+ * Use negative values if the swash plate needs to move down to provide upwards thrust.
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -1
+ * @max 1
  */
-PARAM_DEFINE_INT32(CAL_BARO2_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_HELI_PITCH_C4, 0.45);
 
 /**
- * Barometer 3 priority
+ * Scale for yaw compensation based on collective pitch
  *
+ * This allows to add a proportional factor of the collective pitch command to the yaw command.
+ * A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
+ * 
+ * tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
  * 
  *
- * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
- * @category System
+ * @group Geometry
+ * @decimal 3
+ * @increment 0.1
+ * @min -2
+ * @max 2
  */
-PARAM_DEFINE_INT32(CAL_BARO3_PRIO, -1);
+PARAM_DEFINE_FLOAT(CA_HELI_YAW_CP_S, 0.0);
 
 /**
- * Barometer 0 offset
+ * Offset for yaw compensation based on collective pitch
  *
+ * This allows to specify which collective pitch command results in the least amount of rotor drag.
+ * This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch
+ * by aligning the lowest rotor drag with zero compensation.
+ * For symmetric profile blades this is the command that results in exactly 0° collective blade angle.
+ * For lift profile blades this is typically a command resulting in slightly negative collective blade angle.
+ * 
+ * tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
+ * @increment 0.1
+ * @min -2
+ * @max 2
  */
-PARAM_DEFINE_FLOAT(CAL_BARO0_OFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_HELI_YAW_CP_O, 0.0);
 
 /**
- * Barometer 1 offset
+ * Scale for yaw compensation based on throttle
  *
+ * This allows to add a proportional factor of the throttle command to the yaw command.
+ * A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
+ * 
+ * tail_output += CA_HELI_YAW_TH_S * throttle
  * 
  *
- * @group Sensor Calibration
+ * @group Geometry
  * @decimal 3
- * @category System
- * @volatile True
+ * @increment 0.1
+ * @min -2
+ * @max 2
  */
-PARAM_DEFINE_FLOAT(CAL_BARO1_OFF, 0.0);
+PARAM_DEFINE_FLOAT(CA_HELI_YAW_TH_S, 0.0);
 
 /**
- * Barometer 2 offset
+ * Main rotor turns counter-clockwise
  *
+ * Default configuration is for a clockwise turning main rotor and
+ * positive thrust of the tail rotor is expected to rotate the vehicle clockwise.
+ * Set this parameter to true if the tail rotor provides thrust in counter-clockwise direction
+ * which is mostly the case when the main rotor turns counter-clockwise.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
+ * @group Geometry
+ * @boolean
  */
-PARAM_DEFINE_FLOAT(CAL_BARO2_OFF, 0.0);
+PARAM_DEFINE_INT32(CA_HELI_YAW_CCW, 0);
 
 /**
- * Barometer 3 offset
+ * Motor failure handling mode
  *
+ * This is used to specify how to handle motor failures
+ * reported by failure detector.
  * 
  *
- * @group Sensor Calibration
- * @decimal 3
- * @category System
- * @volatile True
+ * @group Geometry
+ * @value 0 Ignore
+ * @value 1 Remove first failed motor from effectiveness
  */
-PARAM_DEFINE_FLOAT(CAL_BARO3_OFF, 0.0);
+PARAM_DEFINE_INT32(CA_FAILURE_MODE, 0);
 
 /**
- * Gyroscope 0 calibration device ID
+ * Accelerometer 0 calibration device ID
  *
- * Device ID of the gyroscope this calibration applies to.
+ * Device ID of the accelerometer this calibration applies to.
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
+PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
 
 /**
- * Gyroscope 1 calibration device ID
+ * Accelerometer 1 calibration device ID
  *
- * Device ID of the gyroscope this calibration applies to.
+ * Device ID of the accelerometer this calibration applies to.
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
+PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
 
 /**
- * Gyroscope 2 calibration device ID
+ * Accelerometer 2 calibration device ID
  *
- * Device ID of the gyroscope this calibration applies to.
+ * Device ID of the accelerometer this calibration applies to.
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
+PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
 
 /**
- * Gyroscope 3 calibration device ID
+ * Accelerometer 3 calibration device ID
  *
- * Device ID of the gyroscope this calibration applies to.
+ * Device ID of the accelerometer this calibration applies to.
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO3_ID, 0);
+PARAM_DEFINE_INT32(CAL_ACC3_ID, 0);
 
 /**
- * Gyroscope 0 priority
+ * Accelerometer 0 priority
  *
  * 
  *
@@ -6264,12 +6293,13 @@ PARAM_DEFINE_INT32(CAL_GYRO3_ID, 0);
  * @value 50 Medium (Default)
  * @value 75 High
  * @value 100 Max
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1);
 
 /**
- * Gyroscope 1 priority
+ * Accelerometer 1 priority
  *
  * 
  *
@@ -6281,12 +6311,13 @@ PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1);
  * @value 50 Medium (Default)
  * @value 75 High
  * @value 100 Max
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1);
 
 /**
- * Gyroscope 2 priority
+ * Accelerometer 2 priority
  *
  * 
  *
@@ -6298,12 +6329,13 @@ PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1);
  * @value 50 Medium (Default)
  * @value 75 High
  * @value 100 Max
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1);
 
 /**
- * Gyroscope 3 priority
+ * Accelerometer 3 priority
  *
  * 
  *
@@ -6315,12 +6347,13 @@ PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1);
  * @value 50 Medium (Default)
  * @value 75 High
  * @value 100 Max
+ * @decimal 3
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_GYRO3_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_ACC3_PRIO, -1);
 
 /**
- * Gyroscope 0 rotation relative to airframe
+ * Accelerometer 0 rotation relative to airframe
  *
  * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
@@ -6372,10 +6405,10 @@ PARAM_DEFINE_INT32(CAL_GYRO3_PRIO, -1);
  * @min -1
  * @max 40
  */
-PARAM_DEFINE_INT32(CAL_GYRO0_ROT, -1);
+PARAM_DEFINE_INT32(CAL_ACC0_ROT, -1);
 
 /**
- * Gyroscope 1 rotation relative to airframe
+ * Accelerometer 1 rotation relative to airframe
  *
  * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
@@ -6427,10 +6460,10 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ROT, -1);
  * @min -1
  * @max 40
  */
-PARAM_DEFINE_INT32(CAL_GYRO1_ROT, -1);
+PARAM_DEFINE_INT32(CAL_ACC1_ROT, -1);
 
 /**
- * Gyroscope 2 rotation relative to airframe
+ * Accelerometer 2 rotation relative to airframe
  *
  * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
@@ -6482,10 +6515,10 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ROT, -1);
  * @min -1
  * @max 40
  */
-PARAM_DEFINE_INT32(CAL_GYRO2_ROT, -1);
+PARAM_DEFINE_INT32(CAL_ACC2_ROT, -1);
 
 /**
- * Gyroscope 3 rotation relative to airframe
+ * Accelerometer 3 rotation relative to airframe
  *
  * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
@@ -6537,10 +6570,10 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ROT, -1);
  * @min -1
  * @max 40
  */
-PARAM_DEFINE_INT32(CAL_GYRO3_ROT, -1);
+PARAM_DEFINE_INT32(CAL_ACC3_ROT, -1);
 
 /**
- * Gyroscope 0 X-axis offset
+ * Accelerometer 0 X-axis offset
  *
  * 
  *
@@ -6548,12 +6581,12 @@ PARAM_DEFINE_INT32(CAL_GYRO3_ROT, -1);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC0_XOFF, 0.0);
 
 /**
- * Gyroscope 1 X-axis offset
+ * Accelerometer 1 X-axis offset
  *
  * 
  *
@@ -6561,12 +6594,12 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC1_XOFF, 0.0);
 
 /**
- * Gyroscope 2 X-axis offset
+ * Accelerometer 2 X-axis offset
  *
  * 
  *
@@ -6574,12 +6607,12 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC2_XOFF, 0.0);
 
 /**
- * Gyroscope 3 X-axis offset
+ * Accelerometer 3 X-axis offset
  *
  * 
  *
@@ -6587,12 +6620,12 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO3_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC3_XOFF, 0.0);
 
 /**
- * Gyroscope 0 Y-axis offset
+ * Accelerometer 0 Y-axis offset
  *
  * 
  *
@@ -6600,12 +6633,12 @@ PARAM_DEFINE_FLOAT(CAL_GYRO3_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC0_YOFF, 0.0);
 
 /**
- * Gyroscope 1 Y-axis offset
+ * Accelerometer 1 Y-axis offset
  *
  * 
  *
@@ -6613,12 +6646,12 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC1_YOFF, 0.0);
 
 /**
- * Gyroscope 2 Y-axis offset
+ * Accelerometer 2 Y-axis offset
  *
  * 
  *
@@ -6626,12 +6659,147 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @unit m/s^2
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC2_YOFF, 0.0);
+
+/**
+ * Accelerometer 3 Y-axis offset
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC3_YOFF, 0.0);
+
+/**
+ * Accelerometer 0 Z-axis offset
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_ZOFF, 0.0);
+
+/**
+ * Accelerometer 1 Z-axis offset
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC1_ZOFF, 0.0);
+
+/**
+ * Accelerometer 2 Z-axis offset
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_ZOFF, 0.0);
+
+/**
+ * Accelerometer 3 Z-axis offset
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit m/s^2
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC3_ZOFF, 0.0);
+
+/**
+ * Accelerometer 0 X-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_XSCALE, 1.0);
+
+/**
+ * Accelerometer 1 X-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC1_XSCALE, 1.0);
+
+/**
+ * Accelerometer 2 X-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_XSCALE, 1.0);
+
+/**
+ * Accelerometer 3 X-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC3_XSCALE, 1.0);
+
+/**
+ * Accelerometer 0 Y-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0);
 
 /**
- * Gyroscope 3 Y-axis offset
+ * Accelerometer 1 Y-axis scaling factor
  *
  * 
  *
@@ -6639,12 +6807,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0);
 
 /**
- * Gyroscope 0 Z-axis offset
+ * Accelerometer 2 Y-axis scaling factor
  *
  * 
  *
@@ -6652,12 +6821,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0);
 
 /**
- * Gyroscope 1 Z-axis offset
+ * Accelerometer 3 Y-axis scaling factor
  *
  * 
  *
@@ -6665,12 +6835,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC3_YSCALE, 1.0);
 
 /**
- * Gyroscope 2 Z-axis offset
+ * Accelerometer 0 Z-axis scaling factor
  *
  * 
  *
@@ -6678,12 +6849,13 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0);
 
 /**
- * Gyroscope 3 Z-axis offset
+ * Accelerometer 1 Z-axis scaling factor
  *
  * 
  *
@@ -6691,86 +6863,81 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit rad/s
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0);
 
 /**
- * Magnetometer 0 calibration device ID
+ * Accelerometer 2 Z-axis scaling factor
  *
- * Device ID of the magnetometer this calibration applies to.
+ * 
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
+PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0);
 
 /**
- * Magnetometer 1 calibration device ID
+ * Accelerometer 3 Z-axis scaling factor
  *
- * Device ID of the magnetometer this calibration applies to.
+ * 
  *
  * @group Sensor Calibration
+ * @decimal 3
  * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
+PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0);
 
 /**
- * Magnetometer 2 calibration device ID
+ * Barometer 0 calibration device ID
  *
- * Device ID of the magnetometer this calibration applies to.
+ * Device ID of the barometer this calibration applies to.
  *
  * @group Sensor Calibration
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
+PARAM_DEFINE_INT32(CAL_BARO0_ID, 0);
 
 /**
- * Magnetometer 3 calibration device ID
+ * Barometer 1 calibration device ID
  *
- * Device ID of the magnetometer this calibration applies to.
+ * Device ID of the barometer this calibration applies to.
  *
  * @group Sensor Calibration
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
+PARAM_DEFINE_INT32(CAL_BARO1_ID, 0);
 
 /**
- * Magnetometer 0 priority
+ * Barometer 2 calibration device ID
  *
- * 
+ * Device ID of the barometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_BARO2_ID, 0);
 
 /**
- * Magnetometer 1 priority
+ * Barometer 3 calibration device ID
  *
- * 
+ * Device ID of the barometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @value -1 Uninitialized
- * @value 0 Disabled
- * @value 1 Min
- * @value 25 Low
- * @value 50 Medium (Default)
- * @value 75 High
- * @value 100 Max
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_BARO3_ID, 0);
 
 /**
- * Magnetometer 2 priority
+ * Barometer 0 priority
  *
  * 
  *
@@ -6784,10 +6951,10 @@ PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
  * @value 100 Max
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
+PARAM_DEFINE_INT32(CAL_BARO0_PRIO, -1);
 
 /**
- * Magnetometer 3 priority
+ * Barometer 1 priority
  *
  * 
  *
@@ -6801,230 +6968,44 @@ PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
  * @value 100 Max
  * @category System
  */
-PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1);
-
-/**
- * Magnetometer 0 rotation relative to airframe
- *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
- * 
- *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
- */
-PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1);
-
-/**
- * Magnetometer 1 rotation relative to airframe
- *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
- * 
- *
- * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
- * @category System
- * @min -1
- * @max 40
- */
-PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1);
+PARAM_DEFINE_INT32(CAL_BARO1_PRIO, -1);
 
 /**
- * Magnetometer 2 rotation relative to airframe
+ * Barometer 2 priority
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @min -1
- * @max 40
  */
-PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1);
+PARAM_DEFINE_INT32(CAL_BARO2_PRIO, -1);
 
 /**
- * Magnetometer 3 rotation relative to airframe
+ * Barometer 3 priority
  *
- * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @value -1 Internal
- * @value 0 No rotation
- * @value 1 Yaw 45°
- * @value 2 Yaw 90°
- * @value 3 Yaw 135°
- * @value 4 Yaw 180°
- * @value 5 Yaw 225°
- * @value 6 Yaw 270°
- * @value 7 Yaw 315°
- * @value 8 Roll 180°
- * @value 9 Roll 180°, Yaw 45°
- * @value 10 Roll 180°, Yaw 90°
- * @value 11 Roll 180°, Yaw 135°
- * @value 12 Pitch 180°
- * @value 13 Roll 180°, Yaw 225°
- * @value 14 Roll 180°, Yaw 270°
- * @value 15 Roll 180°, Yaw 315°
- * @value 16 Roll 90°
- * @value 17 Roll 90°, Yaw 45°
- * @value 18 Roll 90°, Yaw 90°
- * @value 19 Roll 90°, Yaw 135°
- * @value 20 Roll 270°
- * @value 21 Roll 270°, Yaw 45°
- * @value 22 Roll 270°, Yaw 90°
- * @value 23 Roll 270°, Yaw 135°
- * @value 24 Pitch 90°
- * @value 25 Pitch 270°
- * @value 26 Pitch 180°, Yaw 90°
- * @value 27 Pitch 180°, Yaw 270°
- * @value 28 Roll 90°, Pitch 90°
- * @value 29 Roll 180°, Pitch 90°
- * @value 30 Roll 270°, Pitch 90°
- * @value 31 Roll 90°, Pitch 180°
- * @value 32 Roll 270°, Pitch 180°
- * @value 33 Roll 90°, Pitch 270°
- * @value 34 Roll 180°, Pitch 270°
- * @value 35 Roll 270°, Pitch 270°
- * @value 36 Roll 90°, Pitch 180°, Yaw 90°
- * @value 37 Roll 90°, Yaw 270°
- * @value 38 Roll 90°, Pitch 68°, Yaw 293°
- * @value 39 Pitch 315°
- * @value 40 Roll 90°, Pitch 315°
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @min -1
- * @max 40
  */
-PARAM_DEFINE_INT32(CAL_MAG3_ROT, -1);
+PARAM_DEFINE_INT32(CAL_BARO3_PRIO, -1);
 
 /**
- * Magnetometer 0 X-axis offset
+ * Barometer 0 offset
  *
  * 
  *
@@ -7032,12 +7013,11 @@ PARAM_DEFINE_INT32(CAL_MAG3_ROT, -1);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_BARO0_OFF, 0.0);
 
 /**
- * Magnetometer 1 X-axis offset
+ * Barometer 1 offset
  *
  * 
  *
@@ -7045,12 +7025,11 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_BARO1_OFF, 0.0);
 
 /**
- * Magnetometer 2 X-axis offset
+ * Barometer 2 offset
  *
  * 
  *
@@ -7058,12 +7037,11 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_BARO2_OFF, 0.0);
 
 /**
- * Magnetometer 3 X-axis offset
+ * Barometer 3 offset
  *
  * 
  *
@@ -7071,158 +7049,339 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_XOFF, 0.0);
+PARAM_DEFINE_FLOAT(CAL_BARO3_OFF, 0.0);
 
 /**
- * Magnetometer 0 Y-axis offset
+ * Gyroscope 0 calibration device ID
  *
- * 
+ * Device ID of the gyroscope this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
 
 /**
- * Magnetometer 1 Y-axis offset
+ * Gyroscope 1 calibration device ID
  *
- * 
+ * Device ID of the gyroscope this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
 
 /**
- * Magnetometer 2 Y-axis offset
+ * Gyroscope 2 calibration device ID
  *
- * 
+ * Device ID of the gyroscope this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
 
 /**
- * Magnetometer 3 Y-axis offset
+ * Gyroscope 3 calibration device ID
+ *
+ * Device ID of the gyroscope this calibration applies to.
+ *
+ * @group Sensor Calibration
+ * @category System
+ */
+PARAM_DEFINE_INT32(CAL_GYRO3_ID, 0);
+
+/**
+ * Gyroscope 0 priority
  *
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_YOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1);
 
 /**
- * Magnetometer 0 Z-axis offset
+ * Gyroscope 1 priority
  *
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1);
 
 /**
- * Magnetometer 1 Z-axis offset
+ * Gyroscope 2 priority
  *
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1);
 
 /**
- * Magnetometer 2 Z-axis offset
+ * Gyroscope 3 priority
  *
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
- * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO3_PRIO, -1);
 
 /**
- * Magnetometer 3 Z-axis offset
+ * Gyroscope 0 rotation relative to airframe
  *
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
- * @unit gauss
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_ZOFF, 0.0);
+PARAM_DEFINE_INT32(CAL_GYRO0_ROT, -1);
 
 /**
- * Magnetometer 0 X-axis scaling factor
+ * Gyroscope 1 rotation relative to airframe
  *
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0);
+PARAM_DEFINE_INT32(CAL_GYRO1_ROT, -1);
 
 /**
- * Magnetometer 1 X-axis scaling factor
+ * Gyroscope 2 rotation relative to airframe
  *
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0);
+PARAM_DEFINE_INT32(CAL_GYRO2_ROT, -1);
 
 /**
- * Magnetometer 2 X-axis scaling factor
+ * Gyroscope 3 rotation relative to airframe
  *
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
- * @min 0.1
- * @max 3.0
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0);
+PARAM_DEFINE_INT32(CAL_GYRO3_ROT, -1);
 
 /**
- * Magnetometer 3 X-axis scaling factor
+ * Gyroscope 0 X-axis offset
  *
  * 
  *
@@ -7230,13 +7389,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_XSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO0_XOFF, 0.0);
 
 /**
- * Magnetometer 0 Y-axis scaling factor
+ * Gyroscope 1 X-axis offset
  *
  * 
  *
@@ -7244,13 +7402,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_XSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_XOFF, 0.0);
 
 /**
- * Magnetometer 1 Y-axis scaling factor
+ * Gyroscope 2 X-axis offset
  *
  * 
  *
@@ -7258,13 +7415,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO2_XOFF, 0.0);
 
 /**
- * Magnetometer 2 Y-axis scaling factor
+ * Gyroscope 3 X-axis offset
  *
  * 
  *
@@ -7272,13 +7428,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO3_XOFF, 0.0);
 
 /**
- * Magnetometer 3 Y-axis scaling factor
+ * Gyroscope 0 Y-axis offset
  *
  * 
  *
@@ -7286,13 +7441,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0);
 
 /**
- * Magnetometer 0 Z-axis scaling factor
+ * Gyroscope 1 Y-axis offset
  *
  * 
  *
@@ -7300,13 +7454,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0);
 
 /**
- * Magnetometer 1 Z-axis scaling factor
+ * Gyroscope 2 Y-axis offset
  *
  * 
  *
@@ -7314,13 +7467,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0);
 
 /**
- * Magnetometer 2 Z-axis scaling factor
+ * Gyroscope 3 Y-axis offset
  *
  * 
  *
@@ -7328,13 +7480,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0);
 
 /**
- * Magnetometer 3 Z-axis scaling factor
+ * Gyroscope 0 Z-axis offset
  *
  * 
  *
@@ -7342,13 +7493,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
- * @min 0.1
- * @max 3.0
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0);
 
 /**
- * Magnetometer 0 X-axis off diagonal scale factor
+ * Gyroscope 1 Z-axis offset
  *
  * 
  *
@@ -7356,11 +7506,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0);
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0);
 
 /**
- * Magnetometer 1 X-axis off diagonal scale factor
+ * Gyroscope 2 Z-axis offset
  *
  * 
  *
@@ -7368,11 +7519,12 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0);
 
 /**
- * Magnetometer 2 X-axis off diagonal scale factor
+ * Gyroscope 3 Z-axis offset
  *
  * 
  *
@@ -7380,1176 +7532,1010 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0);
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit rad/s
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0);
+PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0);
 
 /**
- * Magnetometer 3 X-axis off diagonal scale factor
+ * Magnetometer 0 calibration device ID
  *
- * 
+ * Device ID of the magnetometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
 
 /**
- * Magnetometer 0 Y-axis off diagonal scale factor
+ * Magnetometer 1 calibration device ID
  *
- * 
+ * Device ID of the magnetometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
 
 /**
- * Magnetometer 1 Y-axis off diagonal scale factor
+ * Magnetometer 2 calibration device ID
  *
- * 
+ * Device ID of the magnetometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
 
 /**
- * Magnetometer 2 Y-axis off diagonal scale factor
+ * Magnetometer 3 calibration device ID
  *
- * 
+ * Device ID of the magnetometer this calibration applies to.
  *
  * @group Sensor Calibration
- * @decimal 3
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
 
 /**
- * Magnetometer 3 Y-axis off diagonal scale factor
+ * Magnetometer 0 priority
  *
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1);
 
 /**
- * Magnetometer 0 Z-axis off diagonal scale factor
+ * Magnetometer 1 priority
  *
  * 
  *
  * @group Sensor Calibration
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
 
 /**
- * Magnetometer 1 Z-axis off diagonal scale factor
+ * Magnetometer 2 priority
  *
  * 
  *
  * @group Sensor Calibration
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
 
 /**
- * Magnetometer 2 Z-axis off diagonal scale factor
+ * Magnetometer 3 priority
  *
  * 
  *
  * @group Sensor Calibration
+ * @value -1 Uninitialized
+ * @value 0 Disabled
+ * @value 1 Min
+ * @value 25 Low
+ * @value 50 Medium (Default)
+ * @value 75 High
+ * @value 100 Max
  * @category System
- * @volatile True
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1);
 
 /**
- * Magnetometer 3 Z-axis off diagonal scale factor
+ * Magnetometer 0 rotation relative to airframe
  *
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG0_ROT, -1);
 
 /**
- * Magnetometer 0 X Axis throttle compensation
+ * Magnetometer 1 rotation relative to airframe
  *
- * Coefficient describing linear relationship between
- * X component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_XCOMP, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG1_ROT, -1);
 
 /**
- * Magnetometer 1 X Axis throttle compensation
+ * Magnetometer 2 rotation relative to airframe
  *
- * Coefficient describing linear relationship between
- * X component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_XCOMP, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG2_ROT, -1);
 
 /**
- * Magnetometer 2 X Axis throttle compensation
+ * Magnetometer 3 rotation relative to airframe
  *
- * Coefficient describing linear relationship between
- * X component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
+ * An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
  * 
  *
  * @group Sensor Calibration
- * @decimal 3
+ * @value -1 Internal
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 8 Roll 180°
+ * @value 9 Roll 180°, Yaw 45°
+ * @value 10 Roll 180°, Yaw 90°
+ * @value 11 Roll 180°, Yaw 135°
+ * @value 12 Pitch 180°
+ * @value 13 Roll 180°, Yaw 225°
+ * @value 14 Roll 180°, Yaw 270°
+ * @value 15 Roll 180°, Yaw 315°
+ * @value 16 Roll 90°
+ * @value 17 Roll 90°, Yaw 45°
+ * @value 18 Roll 90°, Yaw 90°
+ * @value 19 Roll 90°, Yaw 135°
+ * @value 20 Roll 270°
+ * @value 21 Roll 270°, Yaw 45°
+ * @value 22 Roll 270°, Yaw 90°
+ * @value 23 Roll 270°, Yaw 135°
+ * @value 24 Pitch 90°
+ * @value 25 Pitch 270°
+ * @value 26 Pitch 180°, Yaw 90°
+ * @value 27 Pitch 180°, Yaw 270°
+ * @value 28 Roll 90°, Pitch 90°
+ * @value 29 Roll 180°, Pitch 90°
+ * @value 30 Roll 270°, Pitch 90°
+ * @value 31 Roll 90°, Pitch 180°
+ * @value 32 Roll 270°, Pitch 180°
+ * @value 33 Roll 90°, Pitch 270°
+ * @value 34 Roll 180°, Pitch 270°
+ * @value 35 Roll 270°, Pitch 270°
+ * @value 36 Roll 90°, Pitch 180°, Yaw 90°
+ * @value 37 Roll 90°, Yaw 270°
+ * @value 38 Roll 90°, Pitch 68°, Yaw 293°
+ * @value 39 Pitch 315°
+ * @value 40 Roll 90°, Pitch 315°
  * @category System
- * @volatile True
+ * @min -1
+ * @max 40
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_XCOMP, 0.0);
+PARAM_DEFINE_INT32(CAL_MAG3_ROT, -1);
 
 /**
- * Magnetometer 3 X Axis throttle compensation
+ * Magnetometer 0 X-axis offset
  *
- * Coefficient describing linear relationship between
- * X component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_XCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_XOFF, 0.0);
 
 /**
- * Magnetometer 0 Y Axis throttle compensation
+ * Magnetometer 1 X-axis offset
  *
- * Coefficient describing linear relationship between
- * Y component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG1_XOFF, 0.0);
 
 /**
- * Magnetometer 1 Y Axis throttle compensation
+ * Magnetometer 2 X-axis offset
  *
- * Coefficient describing linear relationship between
- * Y component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG2_XOFF, 0.0);
 
 /**
- * Magnetometer 2 Y Axis throttle compensation
+ * Magnetometer 3 X-axis offset
  *
- * Coefficient describing linear relationship between
- * Y component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG3_XOFF, 0.0);
 
 /**
- * Magnetometer 3 Y Axis throttle compensation
+ * Magnetometer 0 Y-axis offset
  *
- * Coefficient describing linear relationship between
- * Y component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_YOFF, 0.0);
 
 /**
- * Magnetometer 0 Z Axis throttle compensation
+ * Magnetometer 1 Y-axis offset
  *
- * Coefficient describing linear relationship between
- * Z component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG1_YOFF, 0.0);
 
 /**
- * Magnetometer 1 Z Axis throttle compensation
+ * Magnetometer 2 Y-axis offset
  *
- * Coefficient describing linear relationship between
- * Z component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG2_YOFF, 0.0);
 
 /**
- * Magnetometer 2 Z Axis throttle compensation
+ * Magnetometer 3 Y-axis offset
  *
- * Coefficient describing linear relationship between
- * Z component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0);
+PARAM_DEFINE_FLOAT(CAL_MAG3_YOFF, 0.0);
 
 /**
- * Magnetometer 3 Z Axis throttle compensation
+ * Magnetometer 0 Z-axis offset
  *
- * Coefficient describing linear relationship between
- * Z component of magnetometer in body frame axis
- * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
- * Unit for throttle-based compensation is [G] and
- * for current-based compensation [G/kA]
  * 
  *
  * @group Sensor Calibration
  * @decimal 3
  * @category System
  * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0);
-
-/**
- * Enable Gripper actuation in Payload Deliverer
- *
- * 
- *
- * @group Payload Deliverer
- * @boolean
- * @reboot_required True
- */
-PARAM_DEFINE_INT32(PD_GRIPPER_EN, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZOFF, 0.0);
 
 /**
- * Type of Gripper (Servo, etc.)
+ * Magnetometer 1 Z-axis offset
  *
  * 
  *
- * @group Payload Deliverer
- * @value -1 Undefined
- * @value 0 Servo
- * @min -1
- * @max 0
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_INT32(PD_GRIPPER_TYPE, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZOFF, 0.0);
 
 /**
- * Timeout for successful gripper actuation acknowledgement
+ * Magnetometer 2 Z-axis offset
  *
- * Maximum time Gripper will wait while the successful griper actuation isn't recognised.
- * If the gripper has no feedback sensor, it will simply wait for
- * this time before considering gripper actuation successful and publish a
- * 'VehicleCommandAck' signaling successful gripper action
  * 
  *
- * @group Payload Deliverer
- * @min 0
- * @unit s
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(PD_GRIPPER_TO, 3);
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZOFF, 0.0);
 
 /**
- * UAVCAN ESC 1 Output Function
+ * Magnetometer 3 Z-axis offset
  *
- * Select what should be output on UAVCAN ESC 1.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @unit gauss
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC1, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZOFF, 0.0);
 
 /**
- * UAVCAN ESC 2 Output Function
- *
- * Select what should be output on UAVCAN ESC 2.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
- * 
- *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * Magnetometer 0 X-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC2, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_XSCALE, 1.0);
 
 /**
- * UAVCAN ESC 3 Output Function
+ * Magnetometer 1 X-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 3.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_XSCALE, 1.0);
+
+/**
+ * Magnetometer 2 X-axis scaling factor
+ *
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC3, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG2_XSCALE, 1.0);
 
 /**
- * UAVCAN ESC 4 Output Function
+ * Magnetometer 3 X-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 4.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG3_XSCALE, 1.0);
+
+/**
+ * Magnetometer 0 Y-axis scaling factor
+ *
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC4, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0);
 
 /**
- * UAVCAN ESC 5 Output Function
+ * Magnetometer 1 Y-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 5.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0);
+
+/**
+ * Magnetometer 2 Y-axis scaling factor
+ *
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0);
+
+/**
+ * Magnetometer 3 Y-axis scaling factor
+ *
+ * 
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC5, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0);
 
 /**
- * UAVCAN ESC 6 Output Function
+ * Magnetometer 0 Z-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 6.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC6, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0);
 
 /**
- * UAVCAN ESC 7 Output Function
+ * Magnetometer 1 Z-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 7.
- * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC7, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0);
 
 /**
- * UAVCAN ESC 8 Output Function
+ * Magnetometer 2 Z-axis scaling factor
  *
- * Select what should be output on UAVCAN ESC 8.
  * 
- * The default failsafe value is set according to the selected function:
- * - 'Min' for ConstantMin
- * - 'Max' for ConstantMax
- * - 'Max' for Parachute
- * - ('Max'+'Min')/2 for Servos
- * - 'Disarmed' for the rest
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0);
+
+/**
+ * Magnetometer 3 Z-axis scaling factor
+ *
  * 
  *
- * @group Actuator Outputs
- * @value 0 Disabled
- * @value 1 Constant Min
- * @value 2 Constant Max
- * @value 101 Motor 1
- * @value 102 Motor 2
- * @value 103 Motor 3
- * @value 104 Motor 4
- * @value 105 Motor 5
- * @value 106 Motor 6
- * @value 107 Motor 7
- * @value 108 Motor 8
- * @value 109 Motor 9
- * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
+ * @min 0.1
+ * @max 3.0
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FUNC8, 0);
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0);
 
 /**
- * UAVCAN ESC 1 Minimum Value
+ * Magnetometer 0 X-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN1, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0);
 
 /**
- * UAVCAN ESC 2 Minimum Value
+ * Magnetometer 1 X-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN2, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0);
 
 /**
- * UAVCAN ESC 3 Minimum Value
+ * Magnetometer 2 X-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN3, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0);
 
 /**
- * UAVCAN ESC 4 Minimum Value
+ * Magnetometer 3 X-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN4, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0);
 
 /**
- * UAVCAN ESC 5 Minimum Value
+ * Magnetometer 0 Y-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN5, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0);
 
 /**
- * UAVCAN ESC 6 Minimum Value
+ * Magnetometer 1 Y-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN6, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0);
 
 /**
- * UAVCAN ESC 7 Minimum Value
+ * Magnetometer 2 Y-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN7, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0);
 
 /**
- * UAVCAN ESC 8 Minimum Value
+ * Magnetometer 3 Y-axis off diagonal scale factor
  *
- * Minimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MIN8, 1);
+PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0);
 
 /**
- * UAVCAN ESC 1 Maximum Value
+ * Magnetometer 0 Z-axis off diagonal scale factor
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX1, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0);
 
 /**
- * UAVCAN ESC 2 Maximum Value
+ * Magnetometer 1 Z-axis off diagonal scale factor
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX2, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0);
 
 /**
- * UAVCAN ESC 3 Maximum Value
+ * Magnetometer 2 Z-axis off diagonal scale factor
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX3, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0);
 
 /**
- * UAVCAN ESC 4 Maximum Value
+ * Magnetometer 3 Z-axis off diagonal scale factor
  *
- * Maxmimum output value (when not disarmed).
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX4, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0);
 
 /**
- * UAVCAN ESC 5 Maximum Value
+ * Magnetometer 0 X Axis throttle compensation
  *
- * Maxmimum output value (when not disarmed).
+ * Coefficient describing linear relationship between
+ * X component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX5, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG0_XCOMP, 0.0);
 
 /**
- * UAVCAN ESC 6 Maximum Value
+ * Magnetometer 1 X Axis throttle compensation
  *
- * Maxmimum output value (when not disarmed).
+ * Coefficient describing linear relationship between
+ * X component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX6, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG1_XCOMP, 0.0);
 
 /**
- * UAVCAN ESC 7 Maximum Value
+ * Magnetometer 2 X Axis throttle compensation
  *
- * Maxmimum output value (when not disarmed).
+ * Coefficient describing linear relationship between
+ * X component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX7, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG2_XCOMP, 0.0);
 
 /**
- * UAVCAN ESC 8 Maximum Value
+ * Magnetometer 3 X Axis throttle compensation
  *
- * Maxmimum output value (when not disarmed).
+ * Coefficient describing linear relationship between
+ * X component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min 0
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_MAX8, 8191);
+PARAM_DEFINE_FLOAT(CAL_MAG3_XCOMP, 0.0);
 
 /**
- * UAVCAN ESC 1 Failsafe Value
+ * Magnetometer 0 Y Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC1).
+ * Coefficient describing linear relationship between
+ * Y component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL1, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0);
 
 /**
- * UAVCAN ESC 2 Failsafe Value
+ * Magnetometer 1 Y Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC2).
+ * Coefficient describing linear relationship between
+ * Y component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL2, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0);
 
 /**
- * UAVCAN ESC 3 Failsafe Value
+ * Magnetometer 2 Y Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC3).
+ * Coefficient describing linear relationship between
+ * Y component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL3, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0);
 
 /**
- * UAVCAN ESC 4 Failsafe Value
+ * Magnetometer 3 Y Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC4).
+ * Coefficient describing linear relationship between
+ * Y component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL4, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0);
 
 /**
- * UAVCAN ESC 5 Failsafe Value
+ * Magnetometer 0 Z Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC5).
+ * Coefficient describing linear relationship between
+ * Z component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
- *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ *
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL5, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0);
 
 /**
- * UAVCAN ESC 6 Failsafe Value
+ * Magnetometer 1 Z Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC6).
+ * Coefficient describing linear relationship between
+ * Z component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL6, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0);
 
 /**
- * UAVCAN ESC 7 Failsafe Value
+ * Magnetometer 2 Z Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC7).
+ * Coefficient describing linear relationship between
+ * Z component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL7, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0);
 
 /**
- * UAVCAN ESC 8 Failsafe Value
+ * Magnetometer 3 Z Axis throttle compensation
  *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC8).
+ * Coefficient describing linear relationship between
+ * Z component of magnetometer in body frame axis
+ * and either current or throttle depending on value of CAL_MAG_COMP_TYP.
+ * Unit for throttle-based compensation is [G] and
+ * for current-based compensation [G/kA]
  * 
  *
- * @group Actuator Outputs
- * @min -1
- * @max 8191
+ * @group Sensor Calibration
+ * @decimal 3
+ * @category System
+ * @volatile True
  */
-PARAM_DEFINE_INT32(UAVCAN_EC_FAIL8, -1);
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0);
 
 /**
- * UAVCAN Servo 1 Output Function
+ * UAVCAN ESC 1 Output Function
  *
- * Select what should be output on UAVCAN Servo 1.
+ * Select what should be output on UAVCAN ESC 1.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8608,12 +8594,12 @@ PARAM_DEFINE_INT32(UAVCAN_EC_FAIL8, -1);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC1, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC1, 0);
 
 /**
- * UAVCAN Servo 2 Output Function
+ * UAVCAN ESC 2 Output Function
  *
- * Select what should be output on UAVCAN Servo 2.
+ * Select what should be output on UAVCAN ESC 2.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8672,12 +8658,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC1, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC2, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC2, 0);
 
 /**
- * UAVCAN Servo 3 Output Function
+ * UAVCAN ESC 3 Output Function
  *
- * Select what should be output on UAVCAN Servo 3.
+ * Select what should be output on UAVCAN ESC 3.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8736,12 +8722,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC2, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC3, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC3, 0);
 
 /**
- * UAVCAN Servo 4 Output Function
+ * UAVCAN ESC 4 Output Function
  *
- * Select what should be output on UAVCAN Servo 4.
+ * Select what should be output on UAVCAN ESC 4.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8800,12 +8786,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC3, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC4, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC4, 0);
 
 /**
- * UAVCAN Servo 5 Output Function
+ * UAVCAN ESC 5 Output Function
  *
- * Select what should be output on UAVCAN Servo 5.
+ * Select what should be output on UAVCAN ESC 5.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8864,12 +8850,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC4, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC5, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC5, 0);
 
 /**
- * UAVCAN Servo 6 Output Function
+ * UAVCAN ESC 6 Output Function
  *
- * Select what should be output on UAVCAN Servo 6.
+ * Select what should be output on UAVCAN ESC 6.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8928,12 +8914,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC5, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC6, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC6, 0);
 
 /**
- * UAVCAN Servo 7 Output Function
+ * UAVCAN ESC 7 Output Function
  *
- * Select what should be output on UAVCAN Servo 7.
+ * Select what should be output on UAVCAN ESC 7.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -8992,12 +8978,12 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC6, 0);
  * @value 430 Gripper
  * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC7, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC7, 0);
 
 /**
- * UAVCAN Servo 8 Output Function
+ * UAVCAN ESC 8 Output Function
  *
- * Select what should be output on UAVCAN Servo 8.
+ * Select what should be output on UAVCAN ESC 8.
  * 
  * The default failsafe value is set according to the selected function:
  * - 'Min' for ConstantMin
@@ -9021,1299 +9007,1313 @@ PARAM_DEFINE_INT32(UAVCAN_SV_FUNC7, 0);
  * @value 108 Motor 8
  * @value 109 Motor 9
  * @value 110 Motor 10
- * @value 111 Motor 11
- * @value 112 Motor 12
- * @value 201 Servo 1
- * @value 202 Servo 2
- * @value 203 Servo 3
- * @value 204 Servo 4
- * @value 205 Servo 5
- * @value 206 Servo 6
- * @value 207 Servo 7
- * @value 208 Servo 8
- * @value 301 Offboard Actuator Set 1
- * @value 302 Offboard Actuator Set 2
- * @value 303 Offboard Actuator Set 3
- * @value 304 Offboard Actuator Set 4
- * @value 305 Offboard Actuator Set 5
- * @value 306 Offboard Actuator Set 6
- * @value 400 Landing Gear
- * @value 401 Parachute
- * @value 402 RC Roll
- * @value 403 RC Pitch
- * @value 404 RC Throttle
- * @value 405 RC Yaw
- * @value 406 RC Flaps
- * @value 407 RC AUX 1
- * @value 408 RC AUX 2
- * @value 409 RC AUX 3
- * @value 410 RC AUX 4
- * @value 411 RC AUX 5
- * @value 412 RC AUX 6
- * @value 420 Gimbal Roll
- * @value 421 Gimbal Pitch
- * @value 422 Gimbal Yaw
- * @value 430 Gripper
- * @value 440 Landing Gear Wheel
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_FUNC8, 0);
-
-/**
- * UAVCAN Servo 1 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS1, 500);
-
-/**
- * UAVCAN Servo 2 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS2, 500);
-
-/**
- * UAVCAN Servo 3 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS3, 500);
-
-/**
- * UAVCAN Servo 4 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS4, 500);
-
-/**
- * UAVCAN Servo 5 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS5, 500);
-
-/**
- * UAVCAN Servo 6 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS6, 500);
-
-/**
- * UAVCAN Servo 7 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS7, 500);
-
-/**
- * UAVCAN Servo 8 Disarmed Value
- *
- * This is the output value that is set when not armed.
- * 
- * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
- * 
- *
- * @group Actuator Outputs
- * @min 0
- * @max 1000
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_DIS8, 500);
+PARAM_DEFINE_INT32(UAVCAN_EC_FUNC8, 0);
 
 /**
- * UAVCAN Servo 1 Minimum Value
+ * UAVCAN ESC 1 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN1, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN1, 1);
 
 /**
- * UAVCAN Servo 2 Minimum Value
+ * UAVCAN ESC 2 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN2, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN2, 1);
 
 /**
- * UAVCAN Servo 3 Minimum Value
+ * UAVCAN ESC 3 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN3, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN3, 1);
 
 /**
- * UAVCAN Servo 4 Minimum Value
+ * UAVCAN ESC 4 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN4, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN4, 1);
 
 /**
- * UAVCAN Servo 5 Minimum Value
+ * UAVCAN ESC 5 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN5, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN5, 1);
 
 /**
- * UAVCAN Servo 6 Minimum Value
+ * UAVCAN ESC 6 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN6, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN6, 1);
 
 /**
- * UAVCAN Servo 7 Minimum Value
+ * UAVCAN ESC 7 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN7, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN7, 1);
 
 /**
- * UAVCAN Servo 8 Minimum Value
+ * UAVCAN ESC 8 Minimum Value
  *
  * Minimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MIN8, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_MIN8, 1);
 
 /**
- * UAVCAN Servo 1 Maximum Value
+ * UAVCAN ESC 1 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX1, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX1, 8191);
 
 /**
- * UAVCAN Servo 2 Maximum Value
+ * UAVCAN ESC 2 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX2, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX2, 8191);
 
 /**
- * UAVCAN Servo 3 Maximum Value
+ * UAVCAN ESC 3 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX3, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX3, 8191);
 
 /**
- * UAVCAN Servo 4 Maximum Value
+ * UAVCAN ESC 4 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX4, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX4, 8191);
 
 /**
- * UAVCAN Servo 5 Maximum Value
+ * UAVCAN ESC 5 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX5, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX5, 8191);
 
 /**
- * UAVCAN Servo 6 Maximum Value
+ * UAVCAN ESC 6 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX6, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX6, 8191);
 
 /**
- * UAVCAN Servo 7 Maximum Value
+ * UAVCAN ESC 7 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX7, 1000);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX7, 8191);
 
 /**
- * UAVCAN Servo 8 Maximum Value
+ * UAVCAN ESC 8 Maximum Value
  *
  * Maxmimum output value (when not disarmed).
  * 
  *
  * @group Actuator Outputs
  * @min 0
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_MAX8, 1000);
-
-/**
- * UAVCAN Servo 1 Failsafe Value
- *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC1).
- * 
- *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL1, -1);
-
-/**
- * UAVCAN Servo 2 Failsafe Value
- *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC2).
- * 
- *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL2, -1);
-
-/**
- * UAVCAN Servo 3 Failsafe Value
- *
- * This is the output value that is set when in failsafe mode.
- * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC3).
- * 
- *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL3, -1);
+PARAM_DEFINE_INT32(UAVCAN_EC_MAX8, 8191);
 
 /**
- * UAVCAN Servo 4 Failsafe Value
+ * UAVCAN ESC 1 Failsafe Value
  *
  * This is the output value that is set when in failsafe mode.
  * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC4).
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC1).
  * 
  *
  * @group Actuator Outputs
  * @min -1
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL4, -1);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL1, -1);
 
 /**
- * UAVCAN Servo 5 Failsafe Value
+ * UAVCAN ESC 2 Failsafe Value
  *
  * This is the output value that is set when in failsafe mode.
  * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC5).
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC2).
  * 
  *
  * @group Actuator Outputs
  * @min -1
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL5, -1);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL2, -1);
 
 /**
- * UAVCAN Servo 6 Failsafe Value
+ * UAVCAN ESC 3 Failsafe Value
  *
  * This is the output value that is set when in failsafe mode.
  * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC6).
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC3).
  * 
  *
  * @group Actuator Outputs
  * @min -1
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL6, -1);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL3, -1);
 
 /**
- * UAVCAN Servo 7 Failsafe Value
+ * UAVCAN ESC 4 Failsafe Value
  *
  * This is the output value that is set when in failsafe mode.
  * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC7).
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC4).
  * 
  *
  * @group Actuator Outputs
  * @min -1
- * @max 1000
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL7, -1);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL4, -1);
 
 /**
- * UAVCAN Servo 8 Failsafe Value
+ * UAVCAN ESC 5 Failsafe Value
  *
  * This is the output value that is set when in failsafe mode.
  * 
- * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC8).
- * 
- *
- * @group Actuator Outputs
- * @min -1
- * @max 1000
- */
-PARAM_DEFINE_INT32(UAVCAN_SV_FAIL8, -1);
-
-/**
- * Reverse Output Range for UAVCAN
- *
- * Allows to reverse the output range for each channel.
- * Note: this is only useful for servos.
- * 
- *
- * @group Actuator Outputs
- * @bit 0 UAVCAN ESC 1
- * @bit 1 UAVCAN ESC 2
- * @bit 2 UAVCAN ESC 3
- * @bit 3 UAVCAN ESC 4
- * @bit 4 UAVCAN ESC 5
- * @bit 5 UAVCAN ESC 6
- * @bit 6 UAVCAN ESC 7
- * @bit 7 UAVCAN ESC 8
- * @min 0
- * @max 255
- */
-PARAM_DEFINE_INT32(UAVCAN_EC_REV, 0);
-
-/**
- * Reverse Output Range for UAVCAN
- *
- * Allows to reverse the output range for each channel.
- * Note: this is only useful for servos.
- * 
- *
- * @group Actuator Outputs
- * @bit 0 UAVCAN Servo 1
- * @bit 1 UAVCAN Servo 2
- * @bit 2 UAVCAN Servo 3
- * @bit 3 UAVCAN Servo 4
- * @bit 4 UAVCAN Servo 5
- * @bit 5 UAVCAN Servo 6
- * @bit 6 UAVCAN Servo 7
- * @bit 7 UAVCAN Servo 8
- * @min 0
- * @max 255
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC5).
+ * 
+ *
+ * @group Actuator Outputs
+ * @min -1
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UAVCAN_SV_REV, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL5, -1);
 
 /**
- * uXRCE-DDS domain ID
+ * UAVCAN ESC 6 Failsafe Value
  *
- * uXRCE-DDS domain ID
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC6).
+ * 
  *
- * @group UXRCE-DDS Client
- * @category System
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UXRCE_DDS_DOM_ID, 0);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL6, -1);
 
 /**
- * uXRCE-DDS Session key
+ * UAVCAN ESC 7 Failsafe Value
  *
- * uXRCE-DDS key, must be different from zero.
- * In a single agent - multi client configuration, each client
- * must have a unique session key.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC7).
  * 
  *
- * @group UXRCE-DDS Client
- * @category System
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UXRCE_DDS_KEY, 1);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL7, -1);
 
 /**
- * uXRCE-DDS UDP Port
+ * UAVCAN ESC 8 Failsafe Value
  *
- * If ethernet enabled and selected as configuration for uXRCE-DDS,
- * selected udp port will be set and used.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_EC_FUNC8).
  * 
  *
- * @group UXRCE-DDS Client
- * @min 0
- * @max 65535
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 8191
  */
-PARAM_DEFINE_INT32(UXRCE_DDS_PRT, 8888);
+PARAM_DEFINE_INT32(UAVCAN_EC_FAIL8, -1);
 
 /**
- * uXRCE-DDS Agent IP address
+ * UAVCAN Servo 1 Output Function
  *
- * If ethernet enabled and selected as configuration for uXRCE-DDS,
- * selected Agent IP address will be set and used.
- * Decimal dot notation is not supported. IP address must be provided
- * in int32 format. For example, 192.168.1.2 is mapped to -1062731518;
- * 127.0.0.1 is mapped to 2130706433.
+ * Select what should be output on UAVCAN Servo 1.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group UXRCE-DDS Client
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(UXRCE_DDS_AG_IP, 2130706433);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC1, 0);
 
 /**
- * Empty cell voltage (5C load)
+ * UAVCAN Servo 2 Output Function
  *
- * Defines the voltage where a single cell of battery 1 is considered empty.
- * The voltage should be chosen before the steep dropoff to 2.8V. A typical
- * lithium battery can only be discharged down to 10% before it drops off
- * to a voltage level damaging the cells.
+ * Select what should be output on UAVCAN Servo 2.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @unit V
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT1_V_EMPTY, 3.6);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC2, 0);
 
 /**
- * Empty cell voltage (5C load)
+ * UAVCAN Servo 3 Output Function
  *
- * Defines the voltage where a single cell of battery 1 is considered empty.
- * The voltage should be chosen before the steep dropoff to 2.8V. A typical
- * lithium battery can only be discharged down to 10% before it drops off
- * to a voltage level damaging the cells.
+ * Select what should be output on UAVCAN Servo 3.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @unit V
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT2_V_EMPTY, 3.6);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC3, 0);
 
 /**
- * Full cell voltage (5C load)
+ * UAVCAN Servo 4 Output Function
  *
- * Defines the voltage where a single cell of battery 1 is considered full
- * under a mild load. This will never be the nominal voltage of 4.2V
+ * Select what should be output on UAVCAN Servo 4.
  * 
- *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @unit V
- * @reboot_required True
- */
-PARAM_DEFINE_FLOAT(BAT1_V_CHARGED, 4.05);
-
-/**
- * Full cell voltage (5C load)
- *
- * Defines the voltage where a single cell of battery 1 is considered full
- * under a mild load. This will never be the nominal voltage of 4.2V
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @unit V
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT2_V_CHARGED, 4.05);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC4, 0);
 
 /**
- * Voltage drop per cell on full throttle
+ * UAVCAN Servo 5 Output Function
  *
- * This implicitly defines the internal resistance
- * to maximum current ratio for battery 1 and assumes linearity.
- * A good value to use is the difference between the
- * 5C and 20-25C load. Not used if BAT1_R_INTERNAL is
- * set.
+ * Select what should be output on UAVCAN Servo 5.
  * 
- *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @min 0.07
- * @max 0.5
- * @unit V
- * @reboot_required True
- */
-PARAM_DEFINE_FLOAT(BAT1_V_LOAD_DROP, 0.1);
-
-/**
- * Voltage drop per cell on full throttle
- *
- * This implicitly defines the internal resistance
- * to maximum current ratio for battery 1 and assumes linearity.
- * A good value to use is the difference between the
- * 5C and 20-25C load. Not used if BAT2_R_INTERNAL is
- * set.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 2
- * @increment 0.01
- * @min 0.07
- * @max 0.5
- * @unit V
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT2_V_LOAD_DROP, 0.1);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC5, 0);
 
 /**
- * Explicitly defines the per cell internal resistance for battery 1
+ * UAVCAN Servo 6 Output Function
  *
- * If non-negative, then this will be used in place of
- * BAT1_V_LOAD_DROP for all calculations.
+ * Select what should be output on UAVCAN Servo 6.
  * 
- *
- * @group Battery Calibration
- * @decimal 4
- * @increment 0.0005
- * @min -1.0
- * @max 0.2
- * @unit Ohm
- * @reboot_required True
- */
-PARAM_DEFINE_FLOAT(BAT1_R_INTERNAL, 0.005);
-
-/**
- * Explicitly defines the per cell internal resistance for battery 2
- *
- * If non-negative, then this will be used in place of
- * BAT2_V_LOAD_DROP for all calculations.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 4
- * @increment 0.0005
- * @min -1.0
- * @max 0.2
- * @unit Ohm
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT2_R_INTERNAL, 0.005);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC6, 0);
 
 /**
- * Number of cells for battery 1.
+ * UAVCAN Servo 7 Output Function
  *
- * Defines the number of cells the attached battery consists of.
+ * Select what should be output on UAVCAN Servo 7.
  * 
- *
- * @group Battery Calibration
- * @value 1 1S Battery
- * @value 2 2S Battery
- * @value 3 3S Battery
- * @value 4 4S Battery
- * @value 5 5S Battery
- * @value 6 6S Battery
- * @value 7 7S Battery
- * @value 8 8S Battery
- * @value 9 9S Battery
- * @value 10 10S Battery
- * @value 11 11S Battery
- * @value 12 12S Battery
- * @value 13 13S Battery
- * @value 14 14S Battery
- * @value 15 15S Battery
- * @value 16 16S Battery
- * @reboot_required True
- */
-PARAM_DEFINE_INT32(BAT1_N_CELLS, 0);
-
-/**
- * Number of cells for battery 2.
- *
- * Defines the number of cells the attached battery consists of.
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @value 1 1S Battery
- * @value 2 2S Battery
- * @value 3 3S Battery
- * @value 4 4S Battery
- * @value 5 5S Battery
- * @value 6 6S Battery
- * @value 7 7S Battery
- * @value 8 8S Battery
- * @value 9 9S Battery
- * @value 10 10S Battery
- * @value 11 11S Battery
- * @value 12 12S Battery
- * @value 13 13S Battery
- * @value 14 14S Battery
- * @value 15 15S Battery
- * @value 16 16S Battery
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_INT32(BAT2_N_CELLS, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC7, 0);
 
 /**
- * Battery 1 capacity.
+ * UAVCAN Servo 8 Output Function
  *
- * Defines the capacity of battery 1 in mAh.
+ * Select what should be output on UAVCAN Servo 8.
+ * 
+ * The default failsafe value is set according to the selected function:
+ * - 'Min' for ConstantMin
+ * - 'Max' for ConstantMax
+ * - 'Max' for Parachute
+ * - ('Max'+'Min')/2 for Servos
+ * - 'Disarmed' for the rest
  * 
  *
- * @group Battery Calibration
- * @decimal 0
- * @increment 50
- * @min -1.0
- * @max 100000
- * @unit mAh
- * @reboot_required True
+ * @group Actuator Outputs
+ * @value 0 Disabled
+ * @value 1 Constant Min
+ * @value 2 Constant Max
+ * @value 101 Motor 1
+ * @value 102 Motor 2
+ * @value 103 Motor 3
+ * @value 104 Motor 4
+ * @value 105 Motor 5
+ * @value 106 Motor 6
+ * @value 107 Motor 7
+ * @value 108 Motor 8
+ * @value 109 Motor 9
+ * @value 110 Motor 10
+ * @value 111 Motor 11
+ * @value 112 Motor 12
+ * @value 201 Servo 1
+ * @value 202 Servo 2
+ * @value 203 Servo 3
+ * @value 204 Servo 4
+ * @value 205 Servo 5
+ * @value 206 Servo 6
+ * @value 207 Servo 7
+ * @value 208 Servo 8
+ * @value 301 Offboard Actuator Set 1
+ * @value 302 Offboard Actuator Set 2
+ * @value 303 Offboard Actuator Set 3
+ * @value 304 Offboard Actuator Set 4
+ * @value 305 Offboard Actuator Set 5
+ * @value 306 Offboard Actuator Set 6
+ * @value 400 Landing Gear
+ * @value 401 Parachute
+ * @value 402 RC Roll
+ * @value 403 RC Pitch
+ * @value 404 RC Throttle
+ * @value 405 RC Yaw
+ * @value 406 RC Flaps
+ * @value 407 RC AUX 1
+ * @value 408 RC AUX 2
+ * @value 409 RC AUX 3
+ * @value 410 RC AUX 4
+ * @value 411 RC AUX 5
+ * @value 412 RC AUX 6
+ * @value 420 Gimbal Roll
+ * @value 421 Gimbal Pitch
+ * @value 422 Gimbal Yaw
+ * @value 430 Gripper
+ * @value 440 Landing Gear Wheel
  */
-PARAM_DEFINE_FLOAT(BAT1_CAPACITY, -1.0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FUNC8, 0);
 
 /**
- * Battery 2 capacity.
+ * UAVCAN Servo 1 Disarmed Value
  *
- * Defines the capacity of battery 2 in mAh.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Battery Calibration
- * @decimal 0
- * @increment 50
- * @min -1.0
- * @max 100000
- * @unit mAh
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_FLOAT(BAT2_CAPACITY, -1.0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS1, 500);
 
 /**
- * Battery 1 monitoring source.
+ * UAVCAN Servo 2 Disarmed Value
  *
- * This parameter controls the source of battery data. The value 'Power Module'
- * means that measurements are expected to come from a power module. If the value is set to
- * 'External' then the system expects to receive mavlink battery status messages.
- * If the value is set to 'ESCs', the battery information are taken from the esc_status message.
- * This requires the ESC to provide both voltage as well as current.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Battery Calibration
- * @value -1 Disabled
- * @value 0 Power Module
- * @value 1 External
- * @value 2 ESCs
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(BAT1_SOURCE, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS2, 500);
 
 /**
- * Battery 2 monitoring source.
+ * UAVCAN Servo 3 Disarmed Value
  *
- * This parameter controls the source of battery data. The value 'Power Module'
- * means that measurements are expected to come from a power module. If the value is set to
- * 'External' then the system expects to receive mavlink battery status messages.
- * If the value is set to 'ESCs', the battery information are taken from the esc_status message.
- * This requires the ESC to provide both voltage as well as current.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group Battery Calibration
- * @value -1 Disabled
- * @value 0 Power Module
- * @value 1 External
- * @value 2 ESCs
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(BAT2_SOURCE, -1);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS3, 500);
 
 /**
- * MAVLink Config for instance 0
+ * UAVCAN Servo 4 Disarmed Value
  *
- * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group MAVLink
- * @value 0 Disabled
- * @value 1 Uart1
- * @value 2 Uart2
- * @value 3 Uart3
- * @value 4 Uart4
- * @value 5 Uart5
- * @value 6 Uart6
- * @value 7 Uart7
- * @value 8 Uart8
- * @value 20 UBS1
- * @value 21 USB2
- * @value 30 UDP
- * @value 31 TCP
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_CONFIG, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS4, 500);
 
 /**
- * MAVLink Config for instance 1
+ * UAVCAN Servo 5 Disarmed Value
  *
- * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group MAVLink
- * @value 0 Disabled
- * @value 1 Uart1
- * @value 2 Uart2
- * @value 3 Uart3
- * @value 4 Uart4
- * @value 5 Uart5
- * @value 6 Uart6
- * @value 7 Uart7
- * @value 8 Uart8
- * @value 20 UBS1
- * @value 21 USB2
- * @value 30 UDP
- * @value 31 TCP
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_CONFIG, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS5, 500);
 
 /**
- * MAVLink Config for instance 2
+ * UAVCAN Servo 6 Disarmed Value
  *
- * The MAVLink Config defines device(uart/udp/tcp) used by MAVLink instance
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group MAVLink
- * @value 0 Disabled
- * @value 1 Uart1
- * @value 2 Uart2
- * @value 3 Uart3
- * @value 4 Uart4
- * @value 5 Uart5
- * @value 6 Uart6
- * @value 7 Uart7
- * @value 8 Uart8
- * @value 20 UBS1
- * @value 21 USB2
- * @value 30 UDP
- * @value 31 TCP
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_CONFIG, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS6, 500);
 
 /**
- * MAVLink Mode for instance 0
+ * UAVCAN Servo 7 Disarmed Value
  *
- * The MAVLink Mode defines the set of streamed messages (for example the
- * vehicle's attitude) and their sending rates.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group MAVLink
- * @value 0 Normal
- * @value 1 Custom
- * @value 2 Onboard
- * @value 3 OSD
- * @value 4 Magic
- * @value 5 Config
- * @value 7 Minimal
- * @value 8 External Vision
- * @value 10 Gimbal
- * @value 11 Onboard Low Bandwidth
- * @value 12 uAvionix
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_MODE, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS7, 500);
 
 /**
- * MAVLink Mode for instance 1
+ * UAVCAN Servo 8 Disarmed Value
  *
- * The MAVLink Mode defines the set of streamed messages (for example the
- * vehicle's attitude) and their sending rates.
+ * This is the output value that is set when not armed.
+ * 
+ * Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
  * 
  *
- * @group MAVLink
- * @value 0 Normal
- * @value 1 Custom
- * @value 2 Onboard
- * @value 3 OSD
- * @value 4 Magic
- * @value 5 Config
- * @value 7 Minimal
- * @value 8 External Vision
- * @value 10 Gimbal
- * @value 11 Onboard Low Bandwidth
- * @value 12 uAvionix
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_MODE, 2);
+PARAM_DEFINE_INT32(UAVCAN_SV_DIS8, 500);
 
 /**
- * MAVLink Mode for instance 2
+ * UAVCAN Servo 1 Minimum Value
  *
- * The MAVLink Mode defines the set of streamed messages (for example the
- * vehicle's attitude) and their sending rates.
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @value 0 Normal
- * @value 1 Custom
- * @value 2 Onboard
- * @value 3 OSD
- * @value 4 Magic
- * @value 5 Config
- * @value 7 Minimal
- * @value 8 External Vision
- * @value 10 Gimbal
- * @value 11 Onboard Low Bandwidth
- * @value 12 uAvionix
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_MODE, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN1, 0);
 
 /**
- * Maximum MAVLink sending rate for instance 0
+ * UAVCAN Servo 2 Minimum Value
  *
- * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
- * If the configured streams exceed the maximum rate, the sending rate of
- * each stream is automatically decreased.
- * 
- * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
- * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
- * 8N1-configured links).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
+ * @group Actuator Outputs
  * @min 0
- * @unit B/s
- * @reboot_required True
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_RATE, 1200);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN2, 0);
 
 /**
- * Maximum MAVLink sending rate for instance 1
+ * UAVCAN Servo 3 Minimum Value
  *
- * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
- * If the configured streams exceed the maximum rate, the sending rate of
- * each stream is automatically decreased.
- * 
- * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
- * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
- * 8N1-configured links).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
+ * @group Actuator Outputs
  * @min 0
- * @unit B/s
- * @reboot_required True
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_RATE, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN3, 0);
 
 /**
- * Maximum MAVLink sending rate for instance 2
+ * UAVCAN Servo 4 Minimum Value
  *
- * Configure the maximum sending rate for the MAVLink streams in Bytes/sec.
- * If the configured streams exceed the maximum rate, the sending rate of
- * each stream is automatically decreased.
- * 
- * If this is set to 0 a value of half of the theoretical maximum bandwidth is used.
- * This corresponds to baudrate/20 Bytes/s (baudrate/10 = maximum data rate on
- * 8N1-configured links).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
+ * @group Actuator Outputs
  * @min 0
- * @unit B/s
- * @reboot_required True
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_RATE, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN4, 0);
 
 /**
- * Enable MAVLink Message forwarding for instance 0
+ * UAVCAN Servo 5 Minimum Value
  *
- * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
- * message is either broadcast or the target is not the autopilot.
- * 
- * This allows for example a GCS to talk to a camera that is connected to the
- * autopilot via MAVLink (on a different link than the GCS).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_FORWARD, 1);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN5, 0);
 
 /**
- * Enable MAVLink Message forwarding for instance 1
+ * UAVCAN Servo 6 Minimum Value
  *
- * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
- * message is either broadcast or the target is not the autopilot.
- * 
- * This allows for example a GCS to talk to a camera that is connected to the
- * autopilot via MAVLink (on a different link than the GCS).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_FORWARD, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN6, 0);
 
 /**
- * Enable MAVLink Message forwarding for instance 2
+ * UAVCAN Servo 7 Minimum Value
  *
- * If enabled, forward incoming MAVLink messages to other MAVLink ports if the
- * message is either broadcast or the target is not the autopilot.
- * 
- * This allows for example a GCS to talk to a camera that is connected to the
- * autopilot via MAVLink (on a different link than the GCS).
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_FORWARD, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN7, 0);
 
 /**
- * Enable software throttling of mavlink on instance 0
+ * UAVCAN Servo 8 Minimum Value
  *
- * If enabled, MAVLink messages will be throttled according to
- * `txbuf` field reported by radio_status.
- * 
- * Requires a radio to send the mavlink message RADIO_STATUS.
+ * Minimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_RADIO_CTL, 1);
+PARAM_DEFINE_INT32(UAVCAN_SV_MIN8, 0);
 
 /**
- * Enable software throttling of mavlink on instance 1
+ * UAVCAN Servo 1 Maximum Value
  *
- * If enabled, MAVLink messages will be throttled according to
- * `txbuf` field reported by radio_status.
+ * Maxmimum output value (when not disarmed).
  * 
- * Requires a radio to send the mavlink message RADIO_STATUS.
+ *
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX1, 1000);
+
+/**
+ * UAVCAN Servo 2 Maximum Value
+ *
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_RADIO_CTL, 1);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX2, 1000);
 
 /**
- * Enable software throttling of mavlink on instance 2
+ * UAVCAN Servo 3 Maximum Value
  *
- * If enabled, MAVLink messages will be throttled according to
- * `txbuf` field reported by radio_status.
- * 
- * Requires a radio to send the mavlink message RADIO_STATUS.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @boolean
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_RADIO_CTL, 1);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX3, 1000);
 
 /**
- * MAVLink Network Port for instance 0
+ * UAVCAN Servo 4 Maximum Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 0,
- * selected udp port will be set and used in MAVLink instance 0.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_UDP_PRT, 14556);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX4, 1000);
 
 /**
- * MAVLink Network Port for instance 1
+ * UAVCAN Servo 5 Maximum Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 1,
- * selected udp port will be set and used in MAVLink instance 1.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_UDP_PRT, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX5, 1000);
 
 /**
- * MAVLink Network Port for instance 2
+ * UAVCAN Servo 6 Maximum Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 2,
- * selected udp port will be set and used in MAVLink instance 2.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_UDP_PRT, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX6, 1000);
 
 /**
- * MAVLink Remote IP for instance 0
+ * UAVCAN Servo 7 Maximum Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 0,
- * selected remote IP will be set and used in MAVLink instance 0.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_REMOTE_IP, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX7, 1000);
 
 /**
- * MAVLink Remote IP for instance 1
+ * UAVCAN Servo 8 Maximum Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 1,
- * selected remote IP will be set and used in MAVLink instance 1.
+ * Maxmimum output value (when not disarmed).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min 0
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_REMOTE_IP, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_MAX8, 1000);
 
 /**
- * MAVLink Remote IP for instance 2
+ * UAVCAN Servo 1 Failsafe Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 2,
- * selected remote IP will be set and used in MAVLink instance 2.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC1).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_REMOTE_IP, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL1, -1);
 
 /**
- * MAVLink Remote Port for instance 0
+ * UAVCAN Servo 2 Failsafe Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 0,
- * selected remote port will be set and used in MAVLink instance 0.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC2).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_REMOTE_PRT, 14550);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL2, -1);
 
 /**
- * MAVLink Remote Port for instance 1
+ * UAVCAN Servo 3 Failsafe Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 1,
- * selected remote port will be set and used in MAVLink instance 1.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC3).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_REMOTE_PRT, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL3, -1);
 
 /**
- * MAVLink Remote Port for instance 2
+ * UAVCAN Servo 4 Failsafe Value
  *
- * If ethernet enabled and selected as configuration for MAVLink instance 2,
- * selected remote port will be set and used in MAVLink instance 2.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC4).
  * 
  *
- * @group MAVLink
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_REMOTE_PRT, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL4, -1);
 
 /**
- * Broadcast heartbeats on local network for MAVLink instance 0
+ * UAVCAN Servo 5 Failsafe Value
  *
- * This allows a ground control station to automatically find the drone
- * on the local network.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC5).
  * 
  *
- * @group MAVLink
- * @value 0 Never broadcast
- * @value 1 Always broadcast
- * @value 2 Only multicast
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_BROADCAST, 1);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL5, -1);
 
 /**
- * Broadcast heartbeats on local network for MAVLink instance 1
+ * UAVCAN Servo 6 Failsafe Value
  *
- * This allows a ground control station to automatically find the drone
- * on the local network.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC6).
  * 
  *
- * @group MAVLink
- * @value 0 Never broadcast
- * @value 1 Always broadcast
- * @value 2 Only multicast
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_1_BROADCAST, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL6, -1);
 
 /**
- * Broadcast heartbeats on local network for MAVLink instance 2
+ * UAVCAN Servo 7 Failsafe Value
  *
- * This allows a ground control station to automatically find the drone
- * on the local network.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC7).
  * 
  *
- * @group MAVLink
- * @value 0 Never broadcast
- * @value 1 Always broadcast
- * @value 2 Only multicast
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_2_BROADCAST, 0);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL7, -1);
 
 /**
- * Enable serial flow control for instance 0
+ * UAVCAN Servo 8 Failsafe Value
  *
- * This is used to force flow control on or off for the the mavlink
- * instance. By default it is auto detected. Use when auto detection fails.
+ * This is the output value that is set when in failsafe mode.
+ * 
+ * When set to -1 (default), the value depends on the function (see UAVCAN_SV_FUNC8).
  * 
  *
- * @group MAVLink
- * @value 0 Force off
- * @value 1 Force on
- * @value 2 Auto-detected
- * @reboot_required True
+ * @group Actuator Outputs
+ * @min -1
+ * @max 1000
  */
-PARAM_DEFINE_INT32(MAV_0_FLOW_CTRL, 2);
+PARAM_DEFINE_INT32(UAVCAN_SV_FAIL8, -1);
 
 /**
- * Enable serial flow control for instance 1
+ * Reverse Output Range for UAVCAN
  *
- * This is used to force flow control on or off for the the mavlink
- * instance. By default it is auto detected. Use when auto detection fails.
+ * Allows to reverse the output range for each channel.
+ * Note: this is only useful for servos.
  * 
  *
- * @group MAVLink
- * @value 0 Force off
- * @value 1 Force on
- * @value 2 Auto-detected
- * @reboot_required True
+ * @group Actuator Outputs
+ * @bit 0 UAVCAN ESC 1
+ * @bit 1 UAVCAN ESC 2
+ * @bit 2 UAVCAN ESC 3
+ * @bit 3 UAVCAN ESC 4
+ * @bit 4 UAVCAN ESC 5
+ * @bit 5 UAVCAN ESC 6
+ * @bit 6 UAVCAN ESC 7
+ * @bit 7 UAVCAN ESC 8
+ * @min 0
+ * @max 255
  */
-PARAM_DEFINE_INT32(MAV_1_FLOW_CTRL, 2);
+PARAM_DEFINE_INT32(UAVCAN_EC_REV, 0);
 
 /**
- * Enable serial flow control for instance 2
+ * Reverse Output Range for UAVCAN
  *
- * This is used to force flow control on or off for the the mavlink
- * instance. By default it is auto detected. Use when auto detection fails.
+ * Allows to reverse the output range for each channel.
+ * Note: this is only useful for servos.
  * 
  *
- * @group MAVLink
- * @value 0 Force off
- * @value 1 Force on
- * @value 2 Auto-detected
- * @reboot_required True
+ * @group Actuator Outputs
+ * @bit 0 UAVCAN Servo 1
+ * @bit 1 UAVCAN Servo 2
+ * @bit 2 UAVCAN Servo 3
+ * @bit 3 UAVCAN Servo 4
+ * @bit 4 UAVCAN Servo 5
+ * @bit 5 UAVCAN Servo 6
+ * @bit 6 UAVCAN Servo 7
+ * @bit 7 UAVCAN Servo 8
+ * @min 0
+ * @max 255
  */
-PARAM_DEFINE_INT32(MAV_2_FLOW_CTRL, 2);
+PARAM_DEFINE_INT32(UAVCAN_SV_REV, 0);
diff --git a/pkgs/shutdown/external_reset_lockout.cpp b/pkgs/shutdown/external_reset_lockout.cpp
index 63f88d2349..630f20cf38 100644
--- a/pkgs/shutdown/external_reset_lockout.cpp
+++ b/pkgs/shutdown/external_reset_lockout.cpp
@@ -1,62 +1,38 @@
-/****************************************************************************
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
  *
- * Copyright (C) 2020 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.
- *
- ****************************************************************************/
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
 
 #include <px4_platform_common/external_reset_lockout.h>
 
 #if defined(BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE)
 
-#include <px4_platform_common/atomic.h>
+#   include <px4_platform_common/atomic.h>
 
-static px4::atomic<uint8_t> lockout_states {0};
+static px4::atomic<uint8_t> lockout_states{0};
 
-void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled)
-{
-	const uint8_t component_mask = 1 << (uint8_t)component;
-	uint8_t current_state;
+void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) {
+    const uint8_t component_mask = 1 << (uint8_t)component;
+    uint8_t       current_state;
 
-	if (enabled) {
-		current_state = lockout_states.fetch_or(component_mask) | component_mask;
+    if (enabled) {
+        current_state = lockout_states.fetch_or(component_mask) | component_mask;
 
-	} else {
-		current_state = lockout_states.fetch_and(~component_mask) & ~component_mask;
-	}
+    } else {
+        current_state = lockout_states.fetch_and(~component_mask) & ~component_mask;
+    }
 
-	BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(current_state != 0);
+    BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(current_state != 0);
 }
 
 #else
 
-void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) {}
+void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) {
+}
 
 #endif /* BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE */
-