From 277386e6e2888ef127dc93227d492684a5bd1546 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 13 Nov 2024 13:30:29 +0000
Subject: [PATCH] AP_HAL_ChibiOS: add cork()/push() check

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 5da86d9558..5d4274ac70 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1329,6 +1329,9 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
  */
 void RCOutput::cork(void)
 {
+    if (corked) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
     corked = true;
 #if HAL_WITH_IO_MCU
     if (iomcu_enabled) {
@@ -1342,6 +1345,9 @@ void RCOutput::cork(void)
  */
 void RCOutput::push(void)
 {
+    if (!corked) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
     corked = false;
     push_local();
 #if HAL_WITH_IO_MCU