Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 21:07:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[imu] Add heater and temperature options (#2929)

Browse Source
This commit is contained in:
Freek van Tienen
2022-10-27 14:00:13 +02:00
committed by GitHub
parent 6e6bc37d1d
commit 6fa9cdfa53
46 changed files with 733 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+2
View File
@@ -215,12 +215,14 @@
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates *"/>
<field name="samples" type="uint8_t"/>
<field name="temp" type="float" unit="deg Celcius"/>
</message>
<message name="IMU_ACCEL_RAW" id="32">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="accel" type="struct Int32Vect3 *"/>
<field name="samples" type="uint8_t"/>
<field name="temp" type="float" unit="deg Celcius"/>
</message>
<message name="IMU_MAG_RAW" id="33">
conf/airframes/examples/cube_orange.xml
-1
View File
@@ -16,7 +16,6 @@
<firmware name="rotorcraft">
<target name="ap" board="cube_orange">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<!--configure name="RTOS_DEBUG" value="1"/-->
<module name="radio_control" type="sbus">
conf/airframes/tudelft/nederdrone4.xml
+1 -12
View File
@@ -78,6 +78,7 @@
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
</module>
<module name="imu" type="mpu6000"/>
<module name="imu" type="heater"/>
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="indi_simple"/>
@@ -246,21 +247,9 @@
<!-- Calibrated in the MAVLab 14-05-2020 -->
<define name="ACCEL_CALIB" value="{{.abi_id=9, .neutral={-423,18,-28}, .scale={{28111,6328,61706},{6151,1291,12775}}}}"/>
<define name="ACCEL_X_NEUTRAL" value="-423"/>
<define name="ACCEL_Y_NEUTRAL" value="18"/>
<define name="ACCEL_Z_NEUTRAL" value="-28"/>
<define name="ACCEL_X_SENS" value="4.570151198384357" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.901626625187466" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.830215267421878" integer="16"/>
<!-- Calibrated at valkenburg 20-05-2020 (external magnetometer) -->
<define name="MAG_CALIB" value="{{.abi_id=4, .neutral={12,113,49}, .scale={{42598,47669,16336},{3039,3365,1153}}}}"/>
<define name="MAG_X_NEUTRAL" value="12"/>
<define name="MAG_Y_NEUTRAL" value="113"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="14.017110902869321" integer="16"/>
<define name="MAG_Y_SENS" value="14.166121879366417" integer="16"/>
<define name="MAG_Z_SENS" value="14.168256847606495" integer="16"/>
<!-- Define axis in hover frame -->
conf/boards/cube_orange.makefile
+1 -1
View File
@@ -74,7 +74,7 @@ GPS_BAUD ?= B57600
# InterMCU port connected to the IO processor
INTERMCU_PORT ?= UART6
INTERMCU_BAUD ?= B230400
INTERMCU_BAUD ?= B1500000
#
# default actuator configuration
conf/conf_tests.xml
+1 -1
View File
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft
conf/modules/imu_cube.xml
+7 -1
View File
@@ -10,7 +10,7 @@
</description>
</doc>
<dep>
<depends>spi_master,imu_common</depends>
<depends>spi_master,imu_common,intermcu_iomcu,imu_heater</depends>
<provides>imu</provides>
</dep>
<autoload name="imu_nps"/>
@@ -46,6 +46,12 @@
<define name="CUBE_IMU3_SPI_SLAVE_IDX" value="$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
<!-- Configure the heater (ICM20602) -->
<define name="IMU_HEATER_GYRO_ID" value="IMU_CUBE2_ID"/>
<define name="IMU_HEATER_TARGET_TEMP" value="55.0"/>
<define name="IMU_HEATER_P_GAIN" value="50.0"/>
<define name="IMU_HEATER_I_GAIN" value="0.07"/>
<file name="invensense2.c" dir="peripherals"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
conf/modules/imu_heater.xml
+35
View File
@@ -0,0 +1,35 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_heater" dir="imu">
<doc>
<description>
IMU Heater through a resistor or IO co-processor
</description>
<define name="IMU_HEATER_TARGET_TEMP" value="55." description="Target temperature" unit="Celcius"/>
<define name="IMU_HEATER_P_GAIN" value="200." description="kP gain for the heater" unit="%/degC"/>
<define name="IMU_HEATER_I_GAIN" value="0.3" description="kI gain for the heater"/>
<define name="IMU_HEATER_GYRO_ID" value="ABI_BROADCAST" description="Gyro ABI id for the temperature measurement"/>
<define name="IMU_HEATER_ACCEL_ID" description="Accel ABI id for the temperature measurement"/>
<define name="IMU_HEATER_GPIO" description="Heater GPIO port for resistor activation"/>
<define name="IMU_HEATER_GPIO_PIN" description="Heater GPIO pin for resistor activation"/>
<define name="INTERMCU_IOMCU" description="Heater IOMCU communication enabled"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="IMU Heater">
<dl_setting var="imu_heater.target_temp" min="-1" step="0.1" max="70" shortname="target_temp" param="IMU_HEATER_TARGET_TEMP"/>
<dl_setting var="imu_heater.gain_p" min="0" step="0.1" max="500" shortname="gain_p" param="IMU_HEATER_P_GAIN"/>
<dl_setting var="imu_heater.gain_i" min="0" step="0.01" max="10" shortname="gain_i" param="IMU_HEATER_I_GAIN"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="imu_heater.h"/>
</header>
<init fun="imu_heater_init()"/>
<periodic fun="imu_heater_periodic()"/>
<periodic fun="imu_heater_periodic_10hz()" freq="10"/>
<makefile target="!sim|nps|fbw">
<file name="imu_heater.c"/>
</makefile>
</module>
conf/modules/intermcu_iomcu.xml
+27
View File
@@ -0,0 +1,27 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="intermcu_iomcu" dir="intermcu">
<doc>
<description>
InterMCU communication with the ardupilot IO co-processor
</description>
<configure name="INTERMCU_PORT" value="UART6" description="UART used for IO mcu communication"/>
<configure name="INTERMCU_BAUD" value="B1500000" description="UART baud rate"/>
</doc>
<dep>
<depends>uart</depends>
</dep>
<header>
<file name="iomcu.h"/>
</header>
<makefile target="!sim|nps|fbw">
<configure name="INTERMCU_PORT" default="UART6" case="upper|lower"/>
<configure name="INTERMCU_BAUD" default="B1500000"/>
<define name="INTERMCU_LINK" value="$(INTERMCU_PORT_LOWER)"/>
<define name="USE_$(INTERMCU_PORT_UPPER)"/>
<define name="$(INTERMCU_PORT_UPPER)_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="INTERMCU_IOMCU" value="true"/>
<file name="iomcu.c"/>
</makefile>
</module>
conf/telemetry/highspeed_rotorcraft.xml
+2 -1
View File
@@ -58,6 +58,7 @@
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="GPS_INT" period=".2"/>
<message name="IMU_HEATER" period="0.1"/>
</mode>
<mode name="ppm">
@@ -239,7 +240,7 @@
<message name="INDI_G" period="0.1"/>
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<!--<message name="WINDTUNNEL_MEAS" period="0.005"/>-->
<message name="IMU_HEATER" period="1.0"/>
</mode>
</process>
conf/userconf/tudelft/conf.xml
+1 -1
View File
@@ -293,7 +293,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft
sw/airborne/arch/chibios/common_board.h
+8
View File
@@ -81,6 +81,14 @@
#define LED_4_GPIO_OFF gpio_set
#endif
/*
* IMU Heater
*/
#if defined(LINE_IMU_HEATER)
#define IMU_HEATER_GPIO PAL_PORT(LINE_IMU_HEATER)
#define IMU_HEATER_GPIO_PIN PAL_PAD(LINE_IMU_HEATER)
#endif
/*
* ADCs
*/
sw/airborne/boards/apogee/imu_apogee.c
+2 -2
View File
@@ -140,13 +140,13 @@ void imu_apogee_event(void)
(int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Y]),
(int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Z])
};
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1, imu_apogee.mpu.temp);
struct Int32Vect3 accel = {
(int32_t)( imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_X]),
(int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Y]),
(int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Z])
};
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_apogee.mpu.temp);
imu_apogee.mpu.data_available = false;
}
sw/airborne/boards/ardrone/navdata.c
+2 -2
View File
@@ -367,14 +367,14 @@ static void navdata_publish_imu(void)
-navdata.measure.vy,
-navdata.measure.vz
};
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, navdata.measure.temperature_gyro);
struct Int32Vect3 accel = {
navdata.measure.ax,
4096 - navdata.measure.ay,
4096 - navdata.measure.az
};
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, navdata.measure.temperature_acc);
struct Int32Vect3 mag = {
-navdata.measure.mx,
sw/airborne/boards/cube/orange/board.cfg
+2 -2
View File
@@ -93,7 +93,7 @@ PA09 USB_VBUS INPUT PULLDOWN
PA10 UART1_RX UART AF:USART1_RX # IO debug console
PA11 USB_DM OTG AF:USB_OTG_FS_DM
PA12 USB_DP OTG AF:USB_OTG_FS_DP
PA15 ALARM OUTPUT PUSHPULL SPEED_VERYLOW FLOATING LEVEL_HIGH
PA15 ALARM OUTPUT PUSHPULL SPEED_VERYLOW FLOATING LEVEL_LOW
PB00 SPI_SLAVE0_DRDY PASSIVE # EXTERNAL
PB01 SPI_SLAVE0 OUTPUT PUSHPULL SPEED_HIGH FLOATING LEVEL_HIGH # INV EXTERNAL
@@ -163,4 +163,4 @@ PE15 VDD_5V_PERIPH_OC PASSIVE # INV
# GROUPS
GROUP ENERGY_SAVE_INPUTS %NAME=~/^SERVO[0-9]+|LED[0-9]+|SPI_SLAVE[0-9]+$/
GROUP ENERGY_SAVE_LOWS %NAME=~/^VDD_3V3_SENSORS_EN|VDD_5V_PERIPH_EN|PWM_VOLT_SEL$/
GROUP ENERGY_SAVE_LOWS %NAME=~/^VDD_3V3_SENSORS_EN|VDD_5V_PERIPH_EN|PWM_VOLT_SEL|ALARM$/
sw/airborne/boards/cube/orange/board.h
+3 -7
View File
@@ -58,11 +58,6 @@
#define SDLOG_BAT_ADC ADCD1
#define SDLOG_BAT_CHAN AD1_1_CHANNEL
/*
* Include generic board file
*/
//#include "arch/chibios/board.h"
/*
* IO pins assignments.
*/
@@ -448,7 +443,7 @@
PIN_ODR_LEVEL_HIGH(PA12_USB_DP) | \
PIN_ODR_LEVEL_HIGH(PA13_SWDIO) | \
PIN_ODR_LEVEL_HIGH(PA14_SWCLK) | \
PIN_ODR_LEVEL_HIGH(PA15_ALARM))
PIN_ODR_LEVEL_LOW(PA15_ALARM))
#define VAL_GPIOA_AFRL (PIN_AFIO_AF(PA00_UART4_TX, 8) | \
PIN_AFIO_AF(PA01_UART4_RX, 8) | \
@@ -1675,9 +1670,10 @@
#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 3
#define ENERGY_SAVE_LOWS_SIZE 4
#if !defined(_FROM_ASM_)
#ifdef __cplusplus
sw/airborne/boards/px4fmu/chibios/v5.0/board.cfg
+1 -1
View File
@@ -96,7 +96,7 @@ PA03 ADC4 ADC ADC1_IN3 () # BAT2_I
PA04 ADC5 ADC ADC1_IN4 () # ADC1_SPARE2
PA05 FMU_CAP1 PASSIVE
PA06 SPI1_MISO SPI AF:SPI1_MISO
PA07 HEATER PASSIVE
PA07 IMU_HEATER OUTPUT PUSHPULL SPEED_HIGH FLOATING LEVEL_LOW
PA08 CAN3_RX CAN AF:CAN3_RX
PA09 USB_VBUS INPUT PULLDOWN
PA10 SERVO2 PWM AF:TIM1_CH3 ()
sw/airborne/boards/px4fmu/chibios/v5.0/board.h
+8 -8
View File
@@ -75,7 +75,7 @@
#define PA04_ADC5 4U
#define PA05_FMU_CAP1 5U
#define PA06_SPI1_MISO 6U
#define PA07_HEATER 7U
#define PA07_IMU_HEATER 7U
#define PA08_CAN3_RX 8U
#define PA09_USB_VBUS 9U
#define PA10_SERVO2 10U
@@ -265,7 +265,7 @@
#define LINE_ADC5 PAL_LINE(GPIOA, 4U)
#define LINE_FMU_CAP1 PAL_LINE(GPIOA, 5U)
#define LINE_SPI1_MISO PAL_LINE(GPIOA, 6U)
#define LINE_HEATER PAL_LINE(GPIOA, 7U)
#define LINE_IMU_HEATER PAL_LINE(GPIOA, 7U)
#define LINE_CAN3_RX PAL_LINE(GPIOA, 8U)
#define LINE_USB_VBUS PAL_LINE(GPIOA, 9U)
#define LINE_SERVO2 PAL_LINE(GPIOA, 10U)
@@ -436,7 +436,7 @@
PIN_MODE_ANALOG(PA04_ADC5) | \
PIN_MODE_INPUT(PA05_FMU_CAP1) | \
PIN_MODE_ALTERNATE(PA06_SPI1_MISO) | \
PIN_MODE_INPUT(PA07_HEATER) | \
PIN_MODE_OUTPUT(PA07_IMU_HEATER) | \
PIN_MODE_ALTERNATE(PA08_CAN3_RX) | \
PIN_MODE_INPUT(PA09_USB_VBUS) | \
PIN_MODE_ALTERNATE(PA10_SERVO2) | \
@@ -453,7 +453,7 @@
PIN_OTYPE_PUSHPULL(PA04_ADC5) | \
PIN_OTYPE_OPENDRAIN(PA05_FMU_CAP1) | \
PIN_OTYPE_PUSHPULL(PA06_SPI1_MISO) | \
PIN_OTYPE_OPENDRAIN(PA07_HEATER) | \
PIN_OTYPE_PUSHPULL(PA07_IMU_HEATER) | \
PIN_OTYPE_PUSHPULL(PA08_CAN3_RX) | \
PIN_OTYPE_OPENDRAIN(PA09_USB_VBUS) | \
PIN_OTYPE_PUSHPULL(PA10_SERVO2) | \
@@ -470,7 +470,7 @@
PIN_OSPEED_SPEED_VERYLOW(PA04_ADC5) | \
PIN_OSPEED_SPEED_VERYLOW(PA05_FMU_CAP1) | \
PIN_OSPEED_SPEED_HIGH(PA06_SPI1_MISO) | \
PIN_OSPEED_SPEED_VERYLOW(PA07_HEATER) | \
PIN_OSPEED_SPEED_HIGH(PA07_IMU_HEATER) | \
PIN_OSPEED_SPEED_HIGH(PA08_CAN3_RX) | \
PIN_OSPEED_SPEED_VERYLOW(PA09_USB_VBUS) | \
PIN_OSPEED_SPEED_HIGH(PA10_SERVO2) | \
@@ -487,7 +487,7 @@
PIN_PUPDR_FLOATING(PA04_ADC5) | \
PIN_PUPDR_PULLDOWN(PA05_FMU_CAP1) | \
PIN_PUPDR_FLOATING(PA06_SPI1_MISO) | \
PIN_PUPDR_PULLDOWN(PA07_HEATER) | \
PIN_PUPDR_FLOATING(PA07_IMU_HEATER) | \
PIN_PUPDR_FLOATING(PA08_CAN3_RX) | \
PIN_PUPDR_PULLDOWN(PA09_USB_VBUS) | \
PIN_PUPDR_FLOATING(PA10_SERVO2) | \
@@ -504,7 +504,7 @@
PIN_ODR_LEVEL_LOW(PA04_ADC5) | \
PIN_ODR_LEVEL_HIGH(PA05_FMU_CAP1) | \
PIN_ODR_LEVEL_HIGH(PA06_SPI1_MISO) | \
PIN_ODR_LEVEL_HIGH(PA07_HEATER) | \
PIN_ODR_LEVEL_LOW(PA07_IMU_HEATER) | \
PIN_ODR_LEVEL_HIGH(PA08_CAN3_RX) | \
PIN_ODR_LEVEL_LOW(PA09_USB_VBUS) | \
PIN_ODR_LEVEL_LOW(PA10_SERVO2) | \
@@ -521,7 +521,7 @@
PIN_AFIO_AF(PA04_ADC5, 0) | \
PIN_AFIO_AF(PA05_FMU_CAP1, 0) | \
PIN_AFIO_AF(PA06_SPI1_MISO, 5) | \
PIN_AFIO_AF(PA07_HEATER, 0))
PIN_AFIO_AF(PA07_IMU_HEATER, 0))
#define VAL_GPIOA_AFRH (PIN_AFIO_AF(PA08_CAN3_RX, 11) | \
PIN_AFIO_AF(PA09_USB_VBUS, 0) | \
sw/airborne/modules/ahrs/ahrs_vectornav.c
+2 -2
View File
@@ -83,9 +83,9 @@ void ahrs_vectornav_event(void)
uint32_t now_ts = get_sys_time_usec();
// in fixed point for sending as ABI and telemetry msgs
RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1, NAN);
ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1, NAN);
}
}
sw/airborne/modules/imu/imu.c
+8 -6
View File
@@ -162,7 +162,7 @@ PRINT_CONFIG_VAR(IMU_BODY_TO_IMU_PSI)
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
{
static uint8_t id = 0;
pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id,
pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].temperature,
&imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
id++;
if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
@@ -194,7 +194,7 @@ static void send_accel(struct transport_tx *trans, struct link_device *dev)
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
{
static uint8_t id = 0;
pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id,
pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id, &imu.gyros[id].temperature,
&imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
id++;
if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
@@ -273,8 +273,8 @@ static void send_mag_current(struct transport_tx *trans, struct link_device *dev
struct Imu imu = {0};
static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples);
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples);
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp);
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp);
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers);
@@ -489,7 +489,7 @@ void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor,
}
}
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples)
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
{
// Find the correct gyro
struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true);
@@ -549,12 +549,13 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
#endif
// Copy and send
gyro->temperature = temp;
RATES_COPY(gyro->scaled, scaled_rot);
AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
gyro->last_stamp = stamp;
}
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples)
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp)
{
// Find the correct accel
struct imu_accel_t *accel = imu_get_accel(sender_id, true);
@@ -614,6 +615,7 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
#endif
// Copy and send
accel->temperature = temp;
VECT3_COPY(accel->scaled, scaled_rot);
AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
accel->last_stamp = stamp;
sw/airborne/modules/imu/imu.h
+2
View File
@@ -50,6 +50,7 @@ struct imu_gyro_t {
struct imu_calib_t calibrated; ///< Calibration bitmask
struct Int32Rates scaled; ///< Last scaled values in body frame
struct Int32Rates unscaled; ///< Last unscaled values in sensor frame
float temperature; ///< Temperature in degrees celcius
struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
@@ -61,6 +62,7 @@ struct imu_accel_t {
struct imu_calib_t calibrated; ///< Calibration bitmask
struct Int32Vect3 scaled; ///< Last scaled values in body frame
struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
float temperature; ///< Temperature in degrees celcius
struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
sw/airborne/modules/imu/imu_aspirin.c
+2 -2
View File
@@ -139,14 +139,14 @@ void imu_aspirin_event(void)
adxl345_spi_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1, NAN);
imu_aspirin.acc_adxl.data_available = false;
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, NAN);
imu_aspirin.gyro_itg.data_available = false;
}
sw/airborne/modules/imu/imu_aspirin_2_spi.c
+2 -2
View File
@@ -237,8 +237,8 @@ void imu_aspirin2_event(void)
imu_aspirin2.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1, imu_aspirin2.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1, imu_aspirin2.mpu.temp);
#if !ASPIRIN_2_DISABLE_MAG
AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN2_ID, now_ts, &mag_rot);
#endif
sw/airborne/modules/imu/imu_aspirin_i2c.c
+2 -2
View File
@@ -154,14 +154,14 @@ void imu_aspirin_i2c_event(void)
adxl345_i2c_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1, NAN);
imu_aspirin.acc_adxl.data_available = false;
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_aspirin.gyro_itg);
if (imu_aspirin.gyro_itg.data_available) {
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, NAN);
imu_aspirin.gyro_itg.data_available = false;
}
sw/airborne/modules/imu/imu_bebop.c
+2 -2
View File
@@ -131,8 +131,8 @@ void imu_bebop_event(void)
-imu_bebop.mpu.data_accel.vect.z);
imu_bebop.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_bebop.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_bebop.mpu.temp);
}
/* AKM8963 event task */
sw/airborne/modules/imu/imu_bmi088_i2c.c
+2 -2
View File
@@ -125,7 +125,7 @@ void imu_bmi088_event(void)
};
imu_bmi088.bmi.gyro_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1, NAN);
}
if (imu_bmi088.bmi.accel_available) {
// set channel order
@@ -136,7 +136,7 @@ void imu_bmi088_event(void)
};
imu_bmi088.bmi.accel_available = false;
AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1, NAN);
}
}
sw/airborne/modules/imu/imu_cube.c
+2 -2
View File
@@ -126,8 +126,8 @@ void imu_cube_event(void)
imu2.data_available = false;
// Send the scaled values over ABI
AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1, imu2.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1, imu2.temp);
}
invensense2_event(&imu3);
sw/airborne/modules/imu/imu_disco.c
+2 -2
View File
@@ -131,8 +131,8 @@ void imu_disco_event(void)
imu_disco.mpu.data_accel.vect.z);
imu_disco.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_disco.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_disco.mpu.temp);
}
/* AKM8963 event task */
sw/airborne/modules/imu/imu_heater.c
+198
View File
@@ -0,0 +1,198 @@
/*
* Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_heater.c
* IMU heater module which can actuate a resistor heater through GPIO or IOMCU.
*/
#include "imu_heater.h"
#include "mcu_periph/sys_time.h"
#include "modules/core/abi.h"
#include "modules/imu/imu.h"
#include "math/pprz_random.h"
#if INTERMCU_IOMCU
#include "modules/intermcu/iomcu.h"
#endif
/** Default target temperature */
#ifndef IMU_HEATER_TARGET_TEMP
#define IMU_HEATER_TARGET_TEMP 45
#endif
/** Default heater kP gain */
#ifndef IMU_HEATER_P_GAIN
#define IMU_HEATER_P_GAIN 200.0
#endif
/** Default heater kI gain */
#ifndef IMU_HEATER_I_GAIN
#define IMU_HEATER_I_GAIN 0.3
#endif
/** Default heater gyro abi ID */
#if !defined(IMU_HEATER_GYRO_ID) && !defined(IMU_HEATER_ACCEL_ID)
#define IMU_HEATER_GYRO_ID ABI_BROADCAST
#endif
/** Local variables and functions */
struct imu_heater_t imu_heater;
static abi_event imu_heater_abi_ev;
static float imu_heater_run(struct imu_heater_t *heater, float temp);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
/**
* @brief Send the IMU heater message
*
* @param trans Transport structure to send
* @param dev Device to send the message over
*/
static void send_imu_heater(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_IMU_HEATER(trans, dev, AC_ID, &imu_heater.meas_temp, &imu_heater.target_temp, &imu_heater.heat_cmd);
}
#endif
#if defined(IMU_HEATER_GYRO_ID)
static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Rates *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) {
if(isnan(temp))
return;
imu_heater.meas_temp_sum += temp;
imu_heater.meas_temp_cnt++;
}
#endif
#if defined(IMU_HEATER_ACCEL_ID)
static void imu_heater_accel_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) {
if(isnan(temp))
return;
imu_heater.meas_temp_sum += temp;
imu_heater.meas_temp_cnt++;
}
#endif
/**
* @brief Initialize the heater
* Sets the default gains and resets the initial values
*/
void imu_heater_init(void)
{
/* Setup the heater */
imu_heater.target_temp = IMU_HEATER_TARGET_TEMP;
imu_heater.gain_p = IMU_HEATER_P_GAIN;
imu_heater.gain_i = IMU_HEATER_I_GAIN;
imu_heater.meas_temp = NAN;
imu_heater.meas_temp_sum = NAN;
imu_heater.meas_temp_cnt = 0;
imu_heater.integrated = 0;
imu_heater.last_ts = 0;
imu_heater.heat_cmd = 0;
/* Bind to raw temperature measurements */
#if defined(IMU_HEATER_GYRO_ID)
AbiBindMsgIMU_GYRO_RAW(IMU_HEATER_GYRO_ID, &imu_heater_abi_ev, imu_heater_gyro_raw_cb);
#elif defined(IMU_HEATER_ACCEL_ID)
AbiBindMsgIMU_ACCEL_RAW(IMU_HEATER_ACCEL_ID, &imu_heater_abi_ev, imu_heater_accel_raw_cb);
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_HEATER, send_imu_heater);
#endif
}
/**
* @brief High speed heater periodic
* This calculates the temperature average and in case of GPIO heater it
* will provide a heater output. The GPIO out is randomized to reduce the
* magnetometer influence.
*/
void imu_heater_periodic(void)
{
// Set the GPIO heating resistor (using random noise for the magnetometer)
#if defined(IMU_HEATER_GPIO) && defined(IMU_HEATER_GPIO_PIN)
if (imu_heater.heat_cmd != 0 && rand_uniform() * 100 <= imu_heater.heat_cmd) {
gpio_set(IMU_HEATER_GPIO, IMU_HEATER_GPIO_PIN);
} else {
gpio_clear(IMU_HEATER_GPIO, IMU_HEATER_GPIO_PIN);
}
#endif
}
/**
* @brief 10Hz IMU heater periodic
* This will run the control loop for the IMU heater and in case
* of an IOMCU will transmit the duty cycle to the IMU co-processor.
*/
void imu_heater_periodic_10hz(void)
{
// Calculate the command if we have measurements
if (imu_heater.meas_temp_cnt > 0) {
imu_heater.meas_temp = imu_heater.meas_temp_sum / imu_heater.meas_temp_cnt;
imu_heater.heat_cmd = imu_heater_run(&imu_heater, imu_heater.meas_temp);
imu_heater.meas_temp_sum = 0;
imu_heater.meas_temp_cnt = 0;
}
// Send to the IOMCU if needed
#if INTERMCU_IOMCU
iomcu_set_heater_duty_cycle(imu_heater.heat_cmd);
#endif
}
/**
* @brief Calculate the IMU command percentage
*
* @param heater The heater to calculate the command for
* @param temp The current measured temperature
* @return float The target command [0-100%]
*/
static float imu_heater_run(struct imu_heater_t *heater, float temp)
{
if (isnan(temp) || heater->target_temp == -1) {
return 0;
}
// Get the delta time
uint32_t current_ts = get_sys_time_usec();
float dt = (current_ts - heater->last_ts) * 1e-6;
// Calculate the error
float error = heater->target_temp - temp;
// Only do integration if dt is correct
if (heater->last_ts != 0 && dt > 0) {
// Calculate the integrated value and bound to 70%
heater->integrated += heater->gain_i * error * dt;
BoundAbs(heater->integrated, 70);
}
heater->last_ts = current_ts;
// Return the command percentage (0-100% bounded)
float cmd = error * heater->gain_p + heater->integrated;
Bound(cmd, 0, 100);
return cmd;
}
sw/airborne/modules/imu/imu_heater.h
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_heater.h
* IMU heater module which can actuate a resistor heater through GPIO or IOMCU.
*/
#ifndef IMU_HEATER_H
#define IMU_HEATER_H
#include "std.h"
/** Main IMU heater structure */
struct imu_heater_t {
float target_temp; ///< Target temeperature in degrees Celcius
float meas_temp; ///< Measered average temperature in degrees Celcius
float meas_temp_sum; ///< Summed temperature in degrees Celcius
uint32_t meas_temp_cnt; ///< Amount of summed temperatures
float gain_p; ///< Heater kP gain
float gain_i; ///< Heater kI gain
float integrated; ///< Integrated temperature error multiplied by kI (max 70%)
uint32_t last_ts; ///< Last integration timestamp
uint8_t heat_cmd; ///< Heater command 0-100%
};
extern struct imu_heater_t imu_heater;
/** Main external functions */
void imu_heater_init(void);
void imu_heater_periodic(void);
void imu_heater_periodic_10hz(void);
#endif /* IMU_HEATER_H */
sw/airborne/modules/imu/imu_mpu6000.c
+2 -2
View File
@@ -165,7 +165,7 @@ void imu_mpu_spi_event(void)
imu_mpu_spi.mpu.data_available = false;
// Send the scaled values over ABI
AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1, imu_mpu_spi.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1, imu_mpu_spi.mpu.temp);
}
}
sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
+2 -2
View File
@@ -167,8 +167,8 @@ void imu_mpu_hmc_event(void)
};
imu_mpu_hmc.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &rates, 1, imu_mpu_hmc.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1, imu_mpu_hmc.mpu.temp);
}
/* HMC58XX event task */
sw/airborne/modules/imu/imu_mpu60x0_i2c.c
+2 -2
View File
@@ -89,8 +89,8 @@ void imu_mpu_i2c_event(void)
// If the MPU60X0 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_mpu_i2c.mpu);
if (imu_mpu_i2c.mpu.data_available) {
AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1, imu_mpu_i2c.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1, imu_mpu_i2c.mpu.temp);
imu_mpu_i2c.mpu.data_available = false;
}
}
sw/airborne/modules/imu/imu_mpu9250.c
+3 -2
View File
@@ -56,20 +56,21 @@ void imu_mpu9250_event(void)
void imu_mpu9250_report(void)
{
uint8_t id = IMU_MPU9250_ID;
float temp = NAN;
struct Int32Vect3 accel = {
(int32_t)(mpu9250.data_accel.vect.x),
(int32_t)(mpu9250.data_accel.vect.y),
(int32_t)(mpu9250.data_accel.vect.z)
};
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &id, &accel.x, &accel.y, &accel.z);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &id, &accel.x, &accel.y, &accel.z, &temp);
struct Int32Rates rates = {
(int32_t)(mpu9250.data_rates.rates.p),
(int32_t)(mpu9250.data_rates.rates.q),
(int32_t)(mpu9250.data_rates.rates.r)
};
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &id, &rates.p, &rates.q, &rates.r);
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &id, &rates.p, &rates.q, &rates.r, &temp);
struct Int32Vect3 mag = {
(int32_t)(mpu9250.akm.data.vect.x),
sw/airborne/modules/imu/imu_mpu9250_i2c.c
+2 -2
View File
@@ -142,8 +142,8 @@ void imu_mpu9250_event(void)
};
imu_mpu9250.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, NAN);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, NAN);
}
#if IMU_MPU9250_READ_MAG
// Test if mag data are updated
sw/airborne/modules/imu/imu_mpu9250_spi.c
+2 -2
View File
@@ -200,8 +200,8 @@ void imu_mpu9250_event(void)
#endif
imu_mpu9250.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, NAN);
AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, NAN);
}
}
sw/airborne/modules/imu/imu_nps.c
+2 -2
View File
@@ -88,11 +88,11 @@ void imu_nps_event(void)
{
uint32_t now_ts = get_sys_time_usec();
if (imu_nps.gyro_available) {
AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1, NAN);
imu_nps.gyro_available = false;
}
if (imu_nps.accel_available) {
AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1, NAN);
imu_nps.accel_available = false;
}
if (imu_nps.mag_available) {
sw/airborne/modules/imu/imu_px4fmu.c
+2 -2
View File
@@ -103,8 +103,8 @@ void imu_px4fmu_event(void)
-imu_px4fmu.mpu.data_accel.vect.z
};
imu_px4fmu.mpu.data_available = false;
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_px4fmu.mpu.temp);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_px4fmu.mpu.temp);
}
/* HMC58XX event task */
sw/airborne/modules/imu/imu_px4fmu_v2.4.c
+2 -2
View File
@@ -90,7 +90,7 @@ void imu_px4_event(void) {
imu_px4.l3g.data_rates.rates.r
};
imu_px4.l3g.data_available = FALSE;
AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1, NAN);
}
/* LSM303d event task */
@@ -98,7 +98,7 @@ void imu_px4_event(void) {
if (imu_px4.lsm_acc.data_available_acc) {
struct Int32Vect3 accel;
VECT3_COPY(accel, imu_px4.lsm_acc.data_accel.vect);
AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1, NAN);
imu_px4.lsm_acc.data_available_acc = FALSE;
}
#if !IMU_PX4_DISABLE_MAG
sw/airborne/modules/imu/imu_vectornav.c
+2 -3
View File
@@ -148,7 +148,6 @@ void imu_vectornav_propagate(void)
uint32_t now_ts = (uint32_t) tmp;
// Send accel and gyro data separate for other AHRS algorithms
AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1, NAN);
AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1, NAN);
}
sw/airborne/modules/ins/imu_xsens.c
+4 -4
View File
@@ -61,7 +61,7 @@ static void handle_ins_msg(void)
RATE_BFP_OF_REAL(xsens.gyro.r)
};
xsens.gyro_available = FALSE
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN);
}
if (xsens.accel_available) {
struct Int32Vect3 accel = {
@@ -70,7 +70,7 @@ static void handle_ins_msg(void)
ACCEL_BFP_OF_REAL(xsens.accel.z)
};
xsens.accel_available = FALSE;
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN);
}
if (xsens.mag_available) {
struct Int32Vect3 mag = {
@@ -88,7 +88,7 @@ static void handle_ins_msg(void)
RATE_BFP_OF_REAL(xsens.gyro.q),
RATE_BFP_OF_REAL(xsens.gyro.r)
};
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1);
AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN);
xsens.gyro_available = FALSE;
}
if (xsens.accel_available) {
@@ -97,7 +97,7 @@ static void handle_ins_msg(void)
ACCEL_BFP_OF_REAL(xsens.accel.y),
ACCEL_BFP_OF_REAL(xsens.accel.z)
};
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN);
xsens.accel_available = FALSE;
}
if (xsens.mag_available) {
sw/airborne/modules/intermcu/iomcu.c
+277
View File
@@ -0,0 +1,277 @@
/*
* Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/intermcu/iomcu.c
* Driver to communicate with the ardupilot IO MCU
*/
#include "iomcu.h"
#include "mcu_periph/uart.h"
#include <string.h>
// 22 is enough for the rc_input page in one transfer
#define PKT_MAX_REGS 22
#define IOMCU_MAX_CHANNELS 16
struct __attribute__((packed)) IOPacket {
uint8_t count: 6;
uint8_t code: 2;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
};
/*
values for pkt.code
*/
enum iocode {
// read types
CODE_READ = 0,
CODE_WRITE = 1,
// reply codes
CODE_SUCCESS = 0,
CODE_CORRUPT = 1,
CODE_ERROR = 2
};
// IO pages
enum iopage {
PAGE_CONFIG = 0,
PAGE_STATUS = 1,
PAGE_ACTUATORS = 2,
PAGE_SERVOS = 3,
PAGE_RAW_RCIN = 4,
PAGE_RCIN = 5,
PAGE_RAW_ADC = 6,
PAGE_PWM_INFO = 7,
PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_MIXING = 200,
PAGE_GPIO = 201,
};
// setup page registers
#define PAGE_REG_SETUP_FEATURES 0
#define P_SETUP_FEATURES_SBUS1_OUT 1
#define P_SETUP_FEATURES_SBUS2_OUT 2
#define P_SETUP_FEATURES_PWM_RSSI 4
#define P_SETUP_FEATURES_ADC_RSSI 8
#define P_SETUP_FEATURES_ONESHOT 16
#define P_SETUP_FEATURES_BRUSHED 32
#define PAGE_REG_SETUP_ARMING 1
#define P_SETUP_ARMING_IO_ARM_OK (1<<0)
#define P_SETUP_ARMING_FMU_ARMED (1<<1)
#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6)
#define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on
#define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off
#define PAGE_REG_SETUP_PWM_RATE_MASK 2
#define PAGE_REG_SETUP_DEFAULTRATE 3
#define PAGE_REG_SETUP_ALTRATE 4
#define PAGE_REG_SETUP_REBOOT_BL 10
#define PAGE_REG_SETUP_CRC 11
#define PAGE_REG_SETUP_SBUS_RATE 19
#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */
#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21
#define PAGE_REG_SETUP_DSM_BIND 22
#define PAGE_REG_SETUP_RC_PROTOCOLS 23 // uses 2 slots, 23 and 24
// config page registers
#define PAGE_CONFIG_PROTOCOL_VERSION 0
#define PAGE_CONFIG_PROTOCOL_VERSION2 1
#define IOMCU_PROTOCOL_VERSION 4
#define IOMCU_PROTOCOL_VERSION2 10
// magic value for rebooting to bootloader
#define REBOOT_BL_MAGIC 14662
#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12
#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14
#define FORCE_SAFETY_MAGIC 22027
struct page_config {
uint16_t protocol_version;
uint16_t protocol_version2;
};
struct page_reg_status {
uint16_t freemem;
uint32_t timestamp_ms;
uint16_t vservo;
uint16_t vrssi;
uint32_t num_errors;
uint32_t total_pkts;
uint8_t flag_safety_off;
uint8_t err_crc;
uint8_t err_bad_opcode;
uint8_t err_read;
uint8_t err_write;
uint8_t err_uart;
};
struct page_rc_input {
uint8_t count;
uint8_t flags_failsafe: 1;
uint8_t flags_rc_ok: 1;
uint8_t rc_protocol;
uint16_t pwm[IOMCU_MAX_CHANNELS];
int16_t rssi;
};
/*
data for mixing on FMU failsafe
*/
struct page_mixing {
uint16_t servo_min[IOMCU_MAX_CHANNELS];
uint16_t servo_max[IOMCU_MAX_CHANNELS];
uint16_t servo_trim[IOMCU_MAX_CHANNELS];
uint8_t servo_function[IOMCU_MAX_CHANNELS];
uint8_t servo_reversed[IOMCU_MAX_CHANNELS];
// RC input arrays are in AETR order
uint16_t rc_min[4];
uint16_t rc_max[4];
uint16_t rc_trim[4];
uint8_t rc_reversed[IOMCU_MAX_CHANNELS];
uint8_t rc_channel[4];
// gain for elevon and vtail mixing, x1000
uint16_t mixing_gain;
// channel which when high forces mixer
int8_t rc_chan_override;
// is the throttle an angle input?
uint8_t throttle_is_angle;
// mask of channels which are pure manual in override
uint16_t manual_rc_mask;
// enabled needs to be 1 to enable mixing
uint8_t enabled;
uint8_t pad;
};
struct __attribute__((packed, aligned(2))) page_GPIO {
uint8_t channel_mask;
uint8_t output_mask;
};
static const uint8_t crc8_table[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
0xfa, 0xfd, 0xf4, 0xf3
};
/*
crc8 from trone driver by Luis Rodrigues
*/
static uint8_t crc_crc8(const uint8_t *p, uint8_t len)
{
uint16_t crc = 0x0;
while (len--) {
const uint16_t i = (crc ^ *p++) & 0xFF;
crc = (crc8_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}
/**
* @brief Write registers to the IO MCU
*
* @param page The page of the register
* @param offset The offset on the page
* @param count The amount of registers to write
* @param regs The register data
*/
static void iomcu_write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs)
{
// When we want to write more registers do a recursive call of depth 1
while (count > PKT_MAX_REGS) {
iomcu_write_registers(page, offset, PKT_MAX_REGS, regs);
offset += PKT_MAX_REGS;
count -= PKT_MAX_REGS;
regs += PKT_MAX_REGS;
}
// Create the packet
struct IOPacket pkt;
pkt.code = CODE_WRITE;
pkt.count = count;
pkt.page = page;
pkt.offset = offset;
pkt.crc = 0;
memcpy(pkt.regs, regs, 2 * count);
const uint8_t pkt_size = count * 2 + 4;
pkt.crc = crc_crc8((const uint8_t *)&pkt, pkt_size);
// Send the packet
(INTERMCU_LINK).device.put_buffer(&(INTERMCU_LINK), 0, (uint8_t *)&pkt, pkt_size);
}
/**
* @brief Write a single register to the IO MCU
*
* @param page The page of the register
* @param offset The offset on the page
* @param v The value to write to the single register
*/
static void iomcu_write_register(uint8_t page, uint8_t offset, uint16_t v)
{
iomcu_write_registers(page, offset, 1, &v);
}
/**
* @brief Set the IO MCU heater duty cycle
*
* @param duty_cycle The duty cycle to set the heater to [0-100%]
*/
void iomcu_set_heater_duty_cycle(uint8_t duty_cycle)
{
iomcu_write_register(PAGE_SETUP, PAGE_REG_SETUP_HEATER_DUTY_CYCLE, duty_cycle);
}
sw/airborne/modules/intermcu/iomcu.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/intermcu/iomcu.h
* Driver to communicate with the ardupilot IO MCU
*/
#ifndef IOMCU_H
#define IOMCU_H
#include "std.h"
/** External functions */
void iomcu_set_heater_duty_cycle(uint8_t duty_cycle);
#endif /* IOMCU_H */
sw/airborne/peripherals/invensense2.c
+3 -4
View File
@@ -310,13 +310,12 @@ static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *
data += INVENSENSE2_SAMPLE_SIZE;
}
float temp_f = ((float)temp / samples) / 333.87f - 21.f;
float temp_f = ((float)temp / samples) / 333.87f + 21.f;
// Send the scaled values over ABI
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples);
AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j);
AbiSendMsgTEMPERATURE(inv->abi_id, temp_f);
AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples, temp_f);
AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, temp_f);
}
/**
sw/airborne/peripherals/mpu60x0_spi.c
+12 -4
View File
@@ -142,10 +142,18 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
int16_t temp_raw = Int16FromBuf(mpu->rx_buf, 8);
if (mpu->config.type == MPU60X0) {
mpu->temp = (float)temp_raw / 361.0f + 35.0f;
} else {
mpu->temp = (float)temp_raw / 326.8f + 25.0f;
switch(mpu->config.type) {
case ICM20600:
case ICM20602:
case ICM20608:
mpu->temp = (float)temp_raw / 326.8f + 25.0f;
break;
case ICM20689:
mpu->temp = (float)temp_raw * 0.003f + 25.0f;
break;
default:
mpu->temp = (float)temp_raw / 340.0f + 36.53f;
break;
}
// if we are reading slaves, copy the ext_sens_data
sw/airborne/test/peripherals/test_lis302dl_spi.c
+2 -1
View File
@@ -100,8 +100,9 @@ static inline void main_event_task(void)
lis302.data_available = false;
RunOnceEvery(10, {
float temp = NAN;
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
&accel.x, &accel.y, &accel.z);
&accel.x, &accel.y, &accel.z, &temp);
#if USE_LED_6
LED_TOGGLE(6);
#endif
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: afb47d803c...e4e61bb7aa