diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
index ebf4aef07c..fc9066700f 100755
Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ
diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
index ebf4aef07c..fc9066700f 100755
Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ
diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ
diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ
diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
index 9f901fd5ca..25a00e2357 100755
Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ
diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt
index e61ad123be..6e4bcdee66 100644
--- a/src/modules/px4iofirmware/CMakeLists.txt
+++ b/src/modules/px4iofirmware/CMakeLists.txt
@@ -34,13 +34,13 @@
 option(PX4IO_PERF "Enable px4io perf counters" OFF)
 
 add_library(px4iofirmware
-	adc.c
-	controls.c
+	adc.cpp
+	controls.cpp
 	mixer.cpp
-	px4io.c
+	px4io.cpp
 	registers.c
-	safety.c
-	serial.c
+	safety.cpp
+	serial.cpp
 )
 
 set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES px4iofirmware)
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.cpp
similarity index 98%
rename from src/modules/px4iofirmware/adc.c
rename to src/modules/px4iofirmware/adc.cpp
index 1ac0fa2766..1e748e3951 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file adc.c
+ * @file adc.cpp
  *
  * Simple ADC support for PX4IO on STM32.
  */
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.cpp
similarity index 96%
rename from src/modules/px4iofirmware/controls.c
rename to src/modules/px4iofirmware/controls.cpp
index 803fbe6e6c..f5259f62f7 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   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
@@ -131,7 +131,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
 		for (unsigned i = 0; i < n_bytes; i++) {
 			/* set updated flag if one complete packet was parsed */
-			st24_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
+			st24_rssi = input_rc_s::RSSI_MAX;
 			*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
 							    &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 		}
@@ -161,7 +161,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) {
 		for (unsigned i = 0; i < n_bytes; i++) {
 			/* set updated flag if one complete packet was parsed */
-			sumd_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
+			sumd_rssi = input_rc_s::RSSI_MAX;
 			*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
 							    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
 		}
@@ -229,10 +229,10 @@ controls_tick()
 			/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
 			unsigned mV = _rssi_adc_counts * 3300 / 4095;
 			/* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */
-			_rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
+			_rssi = (mV * input_rc_s::RSSI_MAX / 3300);
 
-			if (_rssi > INPUT_RC_RSSI_MAX) {
-				_rssi = INPUT_RC_RSSI_MAX;
+			if (_rssi > input_rc_s::RSSI_MAX) {
+				_rssi = input_rc_s::RSSI_MAX;
 			}
 		}
 	}
@@ -258,11 +258,11 @@ controls_tick()
 		if (sbus_updated) {
 			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
 
-			unsigned sbus_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX
+			unsigned sbus_rssi = input_rc_s::RSSI_MAX;
 
 			if (sbus_frame_drop) {
 				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
-				sbus_rssi = INPUT_RC_RSSI_MAX / 2;
+				sbus_rssi = input_rc_s::RSSI_MAX / 2;
 
 			} else {
 				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.cpp
similarity index 98%
rename from src/modules/px4iofirmware/px4io.c
rename to src/modules/px4iofirmware/px4io.cpp
index 9da4b9431b..74f6637f46 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file px4io.c
+ * @file px4io.cpp
  * Top-level logic for the PX4IO module.
  *
  * @author Lorenz Meier <lorenz@px4.io>
@@ -64,9 +64,7 @@
 #define DEBUG
 #include "px4io.h"
 
-__EXPORT int user_start(int argc, char *argv[]);
-
-struct sys_state_s 	system_state;
+struct sys_state_s system_state;
 
 static struct hrt_call serial_dma_call;
 
@@ -76,7 +74,8 @@ static struct hrt_call serial_dma_call;
 
 static volatile uint32_t msg_counter;
 static volatile uint32_t last_msg_counter;
-static volatile uint8_t msg_next_out, msg_next_in;
+static volatile uint8_t msg_next_out;
+static volatile uint8_t msg_next_in;
 
 /*
  * WARNING: too large buffers here consume the memory required
@@ -275,8 +274,7 @@ calculate_fw_crc(void)
 	r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16;
 }
 
-int
-user_start(int argc, char *argv[])
+extern "C" __EXPORT int user_start(int argc, char *argv[])
 {
 	/* configure the first 8 PWM outputs (i.e. all of them) */
 	up_pwm_servo_init(0xff);
@@ -434,4 +432,3 @@ user_start(int argc, char *argv[])
 		}
 	}
 }
-
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 0ee36d4e71..c8872c8aa0 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -50,6 +50,8 @@
 
 #include "protocol.h"
 
+__BEGIN_DECLS
+
 /*
  * Constants and limits.
  */
@@ -120,8 +122,6 @@ struct sys_state_s {
 };
 
 extern struct sys_state_s system_state;
-extern bool update_mc_thrust_param;
-extern bool update_trims;
 
 # define ENABLE_SBUS_OUT(_s)		px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
 
@@ -185,3 +185,4 @@ extern void	isr_debug(uint8_t level, const char *fmt, ...);
 /** schedule a reboot */
 extern void schedule_reboot(uint32_t time_delta_usec);
 
+__END_DECLS
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index d8166d1dbb..dfe0a6f8ec 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -57,9 +57,6 @@
 static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
 static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
 
-bool update_mc_thrust_param;
-bool update_trims;
-
 /**
  * PAGE 0
  *
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.cpp
similarity index 98%
rename from src/modules/px4iofirmware/safety.c
rename to src/modules/px4iofirmware/safety.cpp
index a28e680f93..1a5eedb7d7 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file safety.c
+ * @file safety.cpp
  * Safety button logic.
  *
  * @author Lorenz Meier <lorenz@px4.io>
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.cpp
similarity index 99%
rename from src/modules/px4iofirmware/serial.c
rename to src/modules/px4iofirmware/serial.cpp
index 55bcfb6633..9829fc2907 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file serial.c
+ * @file serial.cpp
  *
  * Serial communication for the PX4IO module.
  */