Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: support FMU heater pins

Browse Source
This commit is contained in:
Andrew Tridgell 2019-02-22 08:54:42 +11:00
parent 27a1fec911
commit dde97d2b0e
2 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -134,8 +134,8 @@ Util::safety_state Util::safety_switch_state(void)
void Util::set_imu_temp(float current) void Util::set_imu_temp(float current)
{ {
#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER #if HAL_HAVE_IMU_HEATER
if (!heater.target || *heater.target == -1 || !AP_BoardConfig::io_enabled()) { if (!heater.target || *heater.target == -1) {
return; return;
} }
@ -146,6 +146,11 @@ void Util::set_imu_temp(float current)
// update once a second // update once a second
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - heater.last_update_ms < 1000) { if (now - heater.last_update_ms < 1000) {
#if defined(HAL_HEATER_GPIO_PIN)
// output as duty cycle to local pin
hal.gpio->write(HAL_HEATER_GPIO_PIN, heater.duty_counter < heater.output);
heater.duty_counter = (heater.duty_counter+1) % 100;
#endif
return; return;
} }
heater.last_update_ms = now; heater.last_update_ms = now;
@ -167,17 +172,22 @@ void Util::set_imu_temp(float current)
heater.integrator += kI * err; heater.integrator += kI * err;
heater.integrator = constrain_float(heater.integrator, 0, 70); heater.integrator = constrain_float(heater.integrator, 0, 70);
float output = constrain_float(kP * err + heater.integrator, 0, 100); heater.output = constrain_float(kP * err + heater.integrator, 0, 100);
// hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", heater.integrator, output, current, err); //hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", heater.integrator, heater.output, current, err);
iomcu.set_heater_duty_cycle(output); #if HAL_WITH_IO_MCU
#endif // HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER if (AP_BoardConfig::io_enabled()) {
// tell IOMCU to setup heater
iomcu.set_heater_duty_cycle(heater.output);
}
#endif
#endif // HAL_HAVE_IMU_HEATER
} }
void Util::set_imu_target_temp(int8_t *target) void Util::set_imu_target_temp(int8_t *target)
{ {
#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER #if HAL_HAVE_IMU_HEATER
heater.target = target; heater.target = target;
#endif #endif
} }

4
libraries/AP_HAL_ChibiOS/Util.h
View File

@ -78,13 +78,15 @@ private:
void* try_alloc_from_ccm_ram(size_t size); void* try_alloc_from_ccm_ram(size_t size);
uint32_t available_memory_in_ccm_ram(void); uint32_t available_memory_in_ccm_ram(void);
#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER #if HAL_HAVE_IMU_HEATER
struct { struct {
int8_t *target; int8_t *target;
float integrator; float integrator;
uint16_t count; uint16_t count;
float sum; float sum;
uint32_t last_update_ms; uint32_t last_update_ms;
uint8_t duty_counter;
float output;
} heater; } heater;
#endif #endif