AP_IOMCU: remove autodetection of heater pin polarity, instead hardcode it

This commit is contained in:
bugobliterator 2019-07-22 10:49:50 +08:00 committed by Andrew Tridgell
parent bab31a2d61
commit c07fe55b87
3 changed files with 11 additions and 4 deletions

View File

@ -171,8 +171,6 @@ void AP_IOMCU_FW::init()
has_heater = true;
}
//Set Heater PWM Polarity, 0 for Active Low and 1 for Active High
heater_pwm_polarity = !palReadLine(HAL_GPIO_PIN_HEATER);
//Set Heater pin mode
if (heater_pwm_polarity) {
palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_PUSHPULL);

View File

@ -126,7 +126,7 @@ private:
bool update_default_rate;
bool update_rcout_freq;
bool has_heater;
bool heater_pwm_polarity;
const bool heater_pwm_polarity = IOMCU_IMU_HEATER_POLARITY;
uint32_t last_blue_led_ms;
uint32_t safety_update_ms;
uint32_t safety_button_counter;

View File

@ -18,8 +18,17 @@ def build(bld):
'libraries/AP_HAL_ChibiOS/Storage.cpp'
]
)
bld.ap_program(
program_name='iofirmware',
program_name='iofirmware_lowpolh',
use='iofirmware_libs',
program_groups=['bin','iofirmware'],
defines=['IOMCU_IMU_HEATER_POLARITY=0']
)
bld.ap_program(
program_name='iofirmware_highpolh',
use='iofirmware_libs',
program_groups=['bin','iofirmware'],
defines=['IOMCU_IMU_HEATER_POLARITY=1']
)