Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: add min arming temp margin param and getter

Browse Source
This commit is contained in:
Iampete1 2021-10-15 22:22:01 +01:00 committed by Andrew Tridgell
parent 55a6b731a8
commit 3f98927885
3 changed files with 43 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -47,6 +47,10 @@
#define HAL_IMU_TEMP_DEFAULT -1 // disabled
#endif
#ifndef HAL_IMU_TEMP_MARGIN_LOW_DEFAULT
#define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 0 // disabled
#endif
#ifndef BOARD_SAFETY_OPTION_DEFAULT
# define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
#endif
@ -306,6 +310,16 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
AP_GROUPINFO("ALT_CONFIG", 22, AP_BoardConfig, _alt_config, 0),
#endif // HAL_PIN_ALT_CONFIG
#if HAL_HAVE_IMU_HEATER
// @Param: TEMPMGN_LOW
// @DisplayName: hearter temp lower margin
// @Description: Arming check will fail if IMU temp is more than this value lower than BRD_IMU_TARGTEMP, 0 disables
// @Range: 0 20
// @Units: degC
// @User: Advanced
AP_GROUPINFO("TEMPMGN_LOW", 23, AP_BoardConfig, heater.imu_arming_temperature_margin_low, HAL_IMU_TEMP_MARGIN_LOW_DEFAULT),
#endif
AP_GROUPEND
};

6
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -204,6 +204,10 @@ public:
float get_heater_duty_cycle(void) const {
return heater.output;
}
// getters for current temperature and min arming temperature, return false if heater disabled
bool get_board_heater_temperature(float &temperature) const;
bool get_board_heater_arming_temperature(int8_t &temperature) const;
#endif
private:
@ -255,6 +259,8 @@ private:
float sum;
float output;
uint32_t last_log_ms;
float temperature;
AP_Int8 imu_arming_temperature_margin_low;
} heater;
#endif

26
libraries/AP_BoardConfig/IMU_heater.cpp
View File

@ -66,14 +66,14 @@ void AP_BoardConfig::set_imu_temp(float current)
heater.last_update_ms = now;
float avg = heater.sum / heater.count;
heater.temperature = heater.sum / heater.count;
heater.sum = 0;
heater.count = 0;
if (target < 0) {
heater.output = 0;
} else {
heater.output = heater.pi_controller.update(avg, target, dt);
heater.output = heater.pi_controller.update(heater.temperature, target, dt);
heater.output = constrain_float(heater.output, 0, 100);
}
@ -88,7 +88,7 @@ void AP_BoardConfig::set_imu_temp(float current)
// @Field: Out: Controller output to heating element
AP::logger().WriteStreaming("HEAT", "TimeUS,Temp,Targ,P,I,Out", "Qfbfff",
AP_HAL::micros64(),
avg, target,
heater.temperature, target,
heater.pi_controller.get_P(),
heater.pi_controller.get_I(),
heater.output);
@ -111,4 +111,24 @@ void AP_BoardConfig::set_imu_temp(float current)
#endif
}
// getter for current temperature, return false if heater disabled
bool AP_BoardConfig::get_board_heater_temperature(float &temperature) const
{
if (heater.imu_target_temperature == -1) {
return false; // heater disabled
}
temperature = heater.temperature;
return true;
}
// getter for min arming temperature, return false if heater disabled or min check disabled
bool AP_BoardConfig::get_board_heater_arming_temperature(int8_t &temperature) const
{
if ((heater.imu_target_temperature == -1) || (heater.imu_arming_temperature_margin_low == 0)) {
return false; // heater or temperature check disabled
}
temperature = heater.imu_target_temperature - heater.imu_arming_temperature_margin_low;
return true;
}
#endif // HAL_HAVE_IMU_HEATER