Sunk and psi

Tools/AP_Periph/AP_Periph.cpp

@@ -173,6 +173,11 @@ void AP_Periph_FW::init()
 
 #if AP_PERIPH_BARO_ENABLED
     baro.init();
+    // let sensor settle before reading (copied from ardusub)
+    for (uint8_t i = 0; i < 10; i++) {
+        baro.update();
+        hal.scheduler->delay(100);
+    }
 #endif
 
 #if AP_PERIPH_IMU_ENABLED
@@ -323,6 +328,10 @@ void AP_Periph_FW::init()
     actuator_telem.init();
 #endif
 
+#if AP_PERIPH_SUNK_SENSOR_ENABLED
+    sunk_sensor.init();
+#endif
+
     start_ms = AP_HAL::millis();
 }
 
@@ -509,6 +518,13 @@ void AP_Periph_FW::update()
     }
 #endif
 
+#if AP_PERIPH_SUNK_SENSOR_ENABLED
+    if (now - sunk_sensor.last_update_ms >= 100) {
+        sunk_sensor.last_update_ms = now;
+        sunk_sensor.update();
+    }
+#endif
+
 #if AP_PERIPH_RCIN_ENABLED
     rcin_update();
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -41,6 +41,7 @@
 #include "rc_in.h"
 #include "batt_balance.h"
 #include "battery_tag.h"
+#include "sunk_sensor.h"
 #include "battery_bms.h"
 #include "actuator_telem.h"
 #include "networking.h"
@@ -415,6 +416,10 @@ class AP_Periph_FW {
     BatteryTag battery_tag;
 #endif
 
+#if AP_PERIPH_SUNK_SENSOR_ENABLED
+    SunkSensor sunk_sensor;
+#endif
+
 #if AP_PERIPH_BATTERY_BMS_ENABLED
     BatteryBMS battery_bms;
 #endif

Tools/AP_Periph/sunk_sensor.cpp

@@ -0,0 +1,42 @@
+#include "AP_Periph.h"
+
+#if AP_PERIPH_SUNK_SENSOR_ENABLED
+
+#include <dronecan_msgs.h>
+
+extern const AP_HAL::HAL &hal;
+
+uint8_t buffer[1];
+
+void SunkSensor::init(void){
+    hal.gpio->pinMode(HAL_GPIO_SUNK_SENSOR_INPUT, HAL_GPIO_INPUT);
+
+    hal.gpio->write(HAL_GPIO_SUNK_SENSOR_INPUT, 1); //enable with pullup
+
+} 
+
+void SunkSensor::update(void)
+{
+    bool sunk = false;
+    if(hal.gpio->read(HAL_GPIO_SUNK_SENSOR_INPUT)){
+        sunk = true;
+    }
+
+    if(sunk){
+        buffer[0] = SUNFISH_INDICATION_SUNKSTATE_STATUS_SUNK;
+        periph.canard_broadcast(SUNFISH_INDICATION_SUNKSTATE_SIGNATURE,
+            SUNFISH_INDICATION_SUNKSTATE_ID,
+            CANARD_TRANSFER_PRIORITY_LOW,
+            &buffer[0],
+            SUNFISH_INDICATION_SUNKSTATE_MAX_SIZE);
+    }else{
+        buffer[0] = SUNFISH_INDICATION_SUNKSTATE_STATUS_NOT_SUNK;
+        periph.canard_broadcast(SUNFISH_INDICATION_SUNKSTATE_SIGNATURE,
+            SUNFISH_INDICATION_SUNKSTATE_ID,
+            CANARD_TRANSFER_PRIORITY_LOW,
+            &buffer[0],
+            SUNFISH_INDICATION_SUNKSTATE_MAX_SIZE);
+    }
+}
+
+#endif // AP_PERIPH_SUNK_SENSOR_ENABLED

Tools/AP_Periph/sunk_sensor.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#if AP_PERIPH_SUNK_SENSOR_ENABLED
+
+class SunkSensor {
+public:
+    void init(void);
+    void update(void);
+
+    uint32_t last_update_ms;
+};
+
+#endif // AP_PERIPH_SUNK_SENSOR_ENABLED