diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml index 19c85c4553..659b8e794d 100644 --- a/conf/airframes/tudelft/rot_wing_v3d.xml +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -301,6 +301,7 @@
+ diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c index ce8fb0672e..3026444e10 100644 --- a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c +++ b/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( diff --git a/sw/airborne/boards/cube/orange/board.cfg b/sw/airborne/boards/cube/orange/board.cfg index 6856643652..6141a69cfa 100644 --- a/sw/airborne/boards/cube/orange/board.cfg +++ b/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 */ diff --git a/sw/airborne/boards/cube/orange/board.h b/sw/airborne/boards/cube/orange/board.h index e0f739d7c5..1780d78bf0 100644 --- a/sw/airborne/boards/cube/orange/board.h +++ b/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" { diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c index ecff8ce228..422a67d7bc 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c +++ b/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) diff --git a/sw/airborne/modules/energy/electrical.c b/sw/airborne/modules/energy/electrical.c index cef8682960..f0ae795c49 100644 --- a/sw/airborne/modules/energy/electrical.c +++ b/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; diff --git a/sw/airborne/modules/energy/electrical.h b/sw/airborne/modules/energy/electrical.h index 661a626875..6b92fc8c77 100644 --- a/sw/airborne/modules/energy/electrical.h +++ b/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 diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 6d199b814f..19f44a244e 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/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); diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index bd349ff1ee..828de444d7 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit bd349ff1ee713e96a98307d35357ac310c7bddfb +Subproject commit 828de444d7fe6f57d90ef96fa4ca1e62f67bd5a9