Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-06 02:52:42 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[checks] Add IMU heater preflight check and fix minor bugs (#3193)

Browse Source
This commit is contained in:
Freek van Tienen
2023-12-04 09:10:48 +01:00
committed by GitHub
parent cd3c2af7aa
commit 12c65241ab
4 changed files with 48 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
conf/modules/imu_heater.xml
View File

@@ -5,13 +5,16 @@
<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"/>
<section name="IMU_HEATER" prefix="IMU_HEATER_">
<define name="TARGET_TEMP" value="55." description="Target temperature" unit="Celcius"/>
<define name="P_GAIN" value="200." description="kP gain for the heater" unit="%/degC"/>
<define name="I_GAIN" value="0.3" description="kI gain for the heater"/>
<define name="GYRO_ID" value="ABI_BROADCAST" description="Gyro ABI id for the temperature measurement"/>
<define name="ACCEL_ID" description="Accel ABI id for the temperature measurement"/>
<define name="GPIO" description="Heater GPIO port for resistor activation"/>
<define name="GPIO_PIN" description="Heater GPIO pin for resistor activation"/>
<define name="MAX_ERROR" value="0.15" description="Maximum error for the heater preflight check" unit="percentile"/>
</section>
<define name="INTERMCU_IOMCU" description="Heater IOMCU communication enabled"/>
</doc>
<settings>

10
sw/airborne/modules/checks/preflight_checks.c
View File

@@ -30,7 +30,7 @@
/** Maximum combined message size for storing the errors */
#ifndef PREFLIGHT_CHECK_MAX_MSGBUF
#define PREFLIGHT_CHECK_MAX_MSGBUF 512
#define PREFLIGHT_CHECK_MAX_MSGBUF 1024
#endif
/** Seperating character for storing the errors */
@@ -94,10 +94,10 @@ bool preflight_check(void)
// Record the total
int rc = 0;
if (result.fail_cnt > 0) {
rc = snprintf(result.message, result.max_len, "Preflight fail [fail:%d warn:%d tot:%d]", result.fail_cnt,
rc = snprintf(result.message, result.max_len, "*Preflight fail [fail:%d warn:%d tot:%d]*", result.fail_cnt,
result.warning_cnt, (result.fail_cnt + result.warning_cnt + result.success_cnt));
} else {
rc = snprintf(result.message, result.max_len, "Preflight success with warnings [%d/%d]", result.warning_cnt,
rc = snprintf(result.message, result.max_len, "*Preflight success with warnings [%d/%d]*", result.warning_cnt,
(result.fail_cnt + result.warning_cnt + result.success_cnt));
}
if (rc > 0) {
@@ -105,8 +105,8 @@ bool preflight_check(void)
}
// Send the errors seperatly
uint8_t last_sendi = 0;
for (uint8_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
uint16_t last_sendi = 0;
for (uint16_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
if (error_msg[i] == PREFLIGHT_CHECK_SEPERATOR || i == (PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len)) {
DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, i - last_sendi, &error_msg[last_sendi]);
last_sendi = i + 1;

31
sw/airborne/modules/imu/imu_heater.c
View File

@@ -74,6 +74,32 @@ static void send_imu_heater(struct transport_tx *trans, struct link_device *dev)
}
#endif
#ifdef PREFLIGHT_CHECKS
/* Preflight checks */
#include "modules/checks/preflight_checks.h"
static struct preflight_check_t imu_heater_pfc;
/** Maximum error in percentile */
#ifndef IMU_HEATER_MAX_ERROR
#define IMU_HEATER_MAX_ERROR 0.15f
#endif
/** ABI id from either guro or accel */
#if defined(IMU_HEATER_GYRO_ID)
#define IMU_HEATER_ABI_ID IMU_HEATER_GYRO_ID
#elif defined(IMU_HEATER_ACCEL_ID)
#define IMU_HEATER_ABI_ID IMU_HEATER_ACCEL_ID
#endif
static void imu_heater_preflight(struct preflight_result_t *result) {
if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
} else {
preflight_success(result, "IMU %d temperature ok", IMU_HEATER_ABI_ID);
}
}
#endif // PREFLIGHT_CHECKS
#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 rate __attribute__((unused)), float temp) {
if(isnan(temp))
@@ -121,6 +147,11 @@ void imu_heater_init(void)
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_HEATER, send_imu_heater);
#endif
/* Register preflight checks */
#if PREFLIGHT_CHECKS
preflight_check_register(&imu_heater_pfc, imu_heater_preflight);
#endif
}
/**

4
sw/airborne/modules/loggers/sdlog_chibios.c
View File

@@ -114,9 +114,9 @@ static struct preflight_check_t sdlog_pfc;
static void sdlog_preflight(struct preflight_result_t *result) {
if(chibios_sdlog_status != SDLOG_RUNNING) {
#ifdef SDLOG_PREFLIGHT_ERROR
preflight_error(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
preflight_error(result, "SDLogger is not running [%d:%d]", chibios_sdlog_status, sdLogGetStorageStatus());
#else
preflight_warning(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
preflight_warning(result, "SDLogger is not running [%d:%d]", chibios_sdlog_status, sdLogGetStorageStatus());
#endif
} else {
preflight_success(result, "SDLogger running");