Electrical update: add VBoard for CUBE (#3213)

* [electrical] Add board voltage

[energy] Test energy message

[conf] Fix offset

Testing airspeed fixes

[arch] Fix i2c invalidate buffer when reading

* Revert debug

* Default when not defined

* Send VBoard in ROTORCRAFT_STATUS

---------

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>

conf/airframes/tudelft/rot_wing_v3d.xml

@@ -301,6 +301,7 @@
     <section name="MISC">
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
+        <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
 
         <!-- Others -->
         <define name="NO_RC_THRUST_LIMIT"                   value="TRUE"/>

sw/airborne/arch/chibios/mcu_periph/i2c_arch.c

@@ -159,12 +159,12 @@ static void handle_i2c_thd(struct i2c_periph *p)
   } else {
 #if defined(STM32F7XX) || defined(STM32H7XX)
     // we do stupid mem copy because F7 needs a special RAM for DMA operation
-    memcpy(i->dma_buf, (void *)t->buf, (size_t)(t->len_w));
     status = i2cMasterReceiveTimeout(
                (I2CDriver *)p->reg_addr,
                (i2caddr_t)((t->slave_addr) >> 1),
                (uint8_t *)i->dma_buf, (size_t)(t->len_r),
                tmo);
+    cacheBufferInvalidate(i->dma_buf, t->len_r);
     memcpy((void *)t->buf, i->dma_buf, (size_t)(t->len_r));
 #else
     status = i2cMasterReceiveTimeout(

sw/airborne/boards/cube/orange/board.cfg

@@ -31,6 +31,11 @@ HEADER
 #define ADC_CHANNEL_VSUPPLY ADC_1
 #endif
 
+/* allow to define ADC_CHANNEL_VBOARD in the airframe file*/
+#ifndef ADC_CHANNEL_VBOARD
+#define ADC_CHANNEL_VBOARD ADC_3
+#endif
+
 /* allow to define ADC_CHANNEL_CURRENT in the airframe file*/
 #if !defined(ADC_CHANNEL_CURRENT) && !ADC_CURRENT_DISABLE
 #define ADC_CHANNEL_CURRENT ADC_2
@@ -38,6 +43,7 @@ HEADER
 
 /* Default powerbrick values */
 #define DefaultVoltageOfAdc(adc) ((3.3f/65536.0f) * 13.38f * adc)
+#define DefaultVBoardOfAdc(adc) ((3.3f/65536.0f) * 1.89036f * adc)
 #define DefaultMilliAmpereOfAdc(adc) ((3.3f/65536.0f) * 39.877f * adc)
 
 /* Battery monitoring for file closing */

sw/airborne/boards/cube/orange/board.h

@@ -45,6 +45,11 @@
 #define ADC_CHANNEL_VSUPPLY ADC_1
 #endif
 
+/* allow to define ADC_CHANNEL_VBOARD in the airframe file*/
+#ifndef ADC_CHANNEL_VBOARD
+#define ADC_CHANNEL_VBOARD ADC_3
+#endif
+
 /* allow to define ADC_CHANNEL_CURRENT in the airframe file*/
 #if !defined(ADC_CHANNEL_CURRENT) && !ADC_CURRENT_DISABLE
 #define ADC_CHANNEL_CURRENT ADC_2
@@ -52,6 +57,7 @@
 
 /* Default powerbrick values */
 #define DefaultVoltageOfAdc(adc) ((3.3f/65536.0f) * 13.38f * adc)
+#define DefaultVBoardOfAdc(adc) ((3.3f/65536.0f) * 1.89036 * adc)
 #define DefaultMilliAmpereOfAdc(adc) ((3.3f/65536.0f) * 39.877f * adc)
 
 /* Battery monitoring for file closing */
@@ -1661,6 +1667,13 @@
 #define BOARD_GROUP_FOR(array, index, group)  \
   for (ioline_t index=0, *array =  (ioline_t *) group ## _ARRAY; index < group ## _SIZE; index++)
 
+#define ENERGY_SAVE_LOWS \
+	LINE_VDD_5V_PERIPH_EN, \
+	LINE_ALARM, \
+	LINE_PWM_VOLT_SEL, \
+	LINE_VDD_3V3_SENSORS_EN
+#define ENERGY_SAVE_LOWS_SIZE 	 4
+
 #define ENERGY_SAVE_INPUTS \
 	LINE_SPI_SLAVE0, \
 	LINE_SPI_SLAVE1, \
@@ -1680,13 +1693,6 @@
 	LINE_SERVO1
 #define ENERGY_SAVE_INPUTS_SIZE 	 16
 
-#define ENERGY_SAVE_LOWS \
-	LINE_VDD_5V_PERIPH_EN, \
-	LINE_ALARM, \
-	LINE_PWM_VOLT_SEL, \
-	LINE_VDD_3V3_SENSORS_EN
-#define ENERGY_SAVE_LOWS_SIZE 	 4
-
 #if !defined(_FROM_ASM_)
 #ifdef __cplusplus
 extern "C" {

sw/airborne/firmwares/rotorcraft/autopilot_firmware.c

@@ -132,7 +132,7 @@ static void send_status(struct transport_tx *trans, struct link_device *dev)
                                   &radio_control.status, &radio_control.frame_rate,
                                   &fix, &autopilot.mode, &in_flight, &motors_on,
                                   &autopilot.arming_status, &guidance_h.mode, &guidance_v.mode,
-                                  &time_sec, &electrical.vsupply);
+                                  &time_sec, &electrical.vsupply, &electrical.vboard);
 }
 
 static void send_energy(struct transport_tx *trans, struct link_device *dev)

sw/airborne/modules/energy/electrical.c

@@ -69,6 +69,9 @@ PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
 #ifndef VoltageOfAdc
 #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
 #endif
+#ifndef VBoardOfAdc
+#define VBoardOfAdc(adc) DefaultVBoardOfAdc(adc)
+#endif
 #ifndef MilliAmpereOfAdc
 #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
 #endif
@@ -93,6 +96,9 @@ static struct {
 #ifdef ADC_CHANNEL_VSUPPLY
   struct adc_buf vsupply_adc_buf;
 #endif
+#if defined ADC_CHANNEL_VBOARD
+  struct adc_buf vboard_adc_buf;
+#endif
 #if defined ADC_CHANNEL_CURRENT && !defined SITL
   struct adc_buf current_adc_buf;
 #endif
@@ -122,6 +128,7 @@ static void electrical_preflight(struct preflight_result_t *result) {
 void electrical_init(void)
 {
   electrical.vsupply = 0.f;
+  electrical.vboard = 0.f;
   electrical.current = 0.f;
   electrical.charge  = 0.f;
   electrical.energy  = 0.f;
@@ -135,6 +142,10 @@ void electrical_init(void)
   adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
 #endif
 
+#if defined ADC_CHANNEL_VBOARD
+  adc_buf_channel(ADC_CHANNEL_VBOARD, &electrical_priv.vboard_adc_buf, DEFAULT_AV_NB_SAMPLE);
+#endif
+
   /* measure current if available, otherwise estimate it */
 #if defined ADC_CHANNEL_CURRENT && !defined SITL
   adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);
@@ -163,6 +174,11 @@ void electrical_periodic(void)
                                           electrical_priv.vsupply_adc_buf.av_nb_sample));
 #endif
 
+#if defined(ADC_CHANNEL_VBOARD) && !defined(SITL)
+  electrical.vboard = VBoardOfAdc((electrical_priv.vboard_adc_buf.sum /
+                                         electrical_priv.vboard_adc_buf.av_nb_sample));
+#endif
+
 #ifdef ADC_CHANNEL_CURRENT
 #ifndef SITL
   int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample;

sw/airborne/modules/energy/electrical.h

@@ -43,6 +43,7 @@
 
 struct Electrical {
   float vsupply;        ///< supply voltage in V
+  float vboard;         ///< board voltage in V
   float current;        ///< current in A
   float charge;         ///< consumed electric charge in Ah
   float energy;         ///< consumed energy in Wh

sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c

@@ -266,6 +266,19 @@ void ms45xx_i2c_event(void)
        */
 
       float p_out = (p_raw - ms45xx.pressure_offset) * ms45xx.pressure_scale;
+
+      /* 0 = -50degC, 20147 = 150degC
+       * ms45xx_temperature in 0.1 deg Celcius
+       */
+      ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
+      
+      // if(electrical.vboard != 0) {
+      //   float volt_diff = electrical.vboard - 5.0f;
+      //   Bound(volt_diff, -0.7f, 0.5f);
+
+      //   p_out -= 65.0f * volt_diff;
+      //   ms45xx.temperature -= 8.87f * volt_diff;
+      // }
 #ifdef USE_AIRSPEED_LOWPASS_FILTER
       ms45xx.pressure = update_butterworth_2_low_pass(&ms45xx_filter, p_out);
 #else
@@ -285,13 +298,7 @@ void ms45xx_i2c_event(void)
         }
       }
 
-      /* 0 = -50degC, 20147 = 150degC
-       * ms45xx_temperature in 0.1 deg Celcius
-       */
-      ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
-
       // Send (differential) pressure via ABI
-
       #if AIRSPEED_MS45XX_SEND_ABI
       AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.pressure);