diff --git a/Tools/AP_Periph/FSOPowerStack.cpp b/Tools/AP_Periph/FSOPowerStack.cpp index 94dae5fc9774fd..577eee5303094e 100644 --- a/Tools/AP_Periph/FSOPowerStack.cpp +++ b/Tools/AP_Periph/FSOPowerStack.cpp @@ -19,11 +19,18 @@ #include -#define FSO_OUT_VOLTS_DIFF_MAX 2.0 // Maximum difference between output and batteries before turn on -#define FSO_BATT_VOLTS_DIFF_MAX 1.0 // Maximum difference between batteries before turn on -#define FSO_PRECHARGE_TIMEOUT_MS 500 // Maximum pre-charge time before turn on is aborted -#define FSO_SWITCH_ON_TIME_MS 500 // Minimum press time to turn on -#define FSO_SWITCH_OFF_TIME_MS 1000 // Minimum press time to turn off +#define FSO_OUT_VOLTS_DIFF_MAX 2.0 // Maximum difference between output and batteries before turn on +#define FSO_BATT_VOLTS_DIFF_MAX 1.0 // Maximum difference between batteries before turn on +#define FSO_PRECHARGE_TIMEOUT_MS 500 // Maximum pre-charge time before turn on is aborted +#define FSO_SWITCH_ON_TIME_MS 500 // Minimum press time to turn on +#define FSO_SWITCH_OFF_TIME_MS 1000 // Minimum press time to turn off +#define FSO_LOOP_TIME_MS 100 // Loop time in ms +#define FSO_PAYLOAD_HV_CURRENT_MAX 25.0 // Maximum current of the HV payload output +#define FSO_PAYLOAD_BEC_CURRENT_MAX 10.0 // Maximum current of the payload BEC's +#define FSO_PAYLOAD_BEC_TEMPERATURE_MAX 100.0 // Maximum temperature of the payload BEC's +#define FSO_INTERNAL_BEC_HC_CURRENT_MAX 10.0 // Maximum current of the high current internal BEC +#define FSO_INTERNAL_BEC_CURRENT_MAX 1.0 // Maximum current of the low current internal BEC's +#define FSO_INTERNAL_BEC_TEMPERATURE_MAX 100.0 // Maximum temperature of the payload BEC's extern const AP_HAL::HAL &hal; @@ -34,17 +41,65 @@ const AP_Param::GroupInfo FSOPowerStack::var_info[] { // @Bitmask: 0:SomeOption AP_GROUPINFO("_OPTIONS", 1, FSOPowerStack, options, 0), - // @Param: _P1_VOLT + // @Param: _HV_AMP_LIM + // @DisplayName: Payload HV amp limit + // @Description: Payload HV amp limit + // @Range: 0 25 + AP_GROUPINFO("_HV_AMP_LIM", 2, FSOPowerStack, payload_HV_current_max, FSO_PAYLOAD_HV_CURRENT_MAX), + + // @Param: _BEC1_VOLT // @DisplayName: Payload 1 voltage // @Description: Payload 1 voltage // @Range: 0 100 - AP_GROUPINFO("_P1_VOLT", 2, FSOPowerStack, p1_voltage, 0), + AP_GROUPINFO("_BEC1_VOLT", 3, FSOPowerStack, payload_1_voltage, 5.0), - // @Param: _P2_VOLT + // @Param: _BEC2_VOLT // @DisplayName: Payload 2 voltage // @Description: Payload 2 voltage // @Range: 0 100 - AP_GROUPINFO("_P2_VOLT", 3, FSOPowerStack, p2_voltage, 0), + AP_GROUPINFO("_BEC2_VOLT", 4, FSOPowerStack, payload_2_voltage, 12.0), + + // @Param: _BEC1_AMP_LIM + // @DisplayName: Payload 1 amp limit + // @Description: Payload 1 amp limit + // @Range: 0 10 + AP_GROUPINFO("_BEC1_AMP_LIM", 5, FSOPowerStack, payload_1_current_max, FSO_PAYLOAD_BEC_CURRENT_MAX), + + // @Param: _BEC2_AMP_LIM + // @DisplayName: Payload 2 amp limit + // @Description: Payload 2 amp limit + // @Range: 0 10 + AP_GROUPINFO("_BEC2_AMP_LIM", 6, FSOPowerStack, payload_2_current_max, FSO_PAYLOAD_BEC_CURRENT_MAX), + + // @Param: _BEC_TEMP_LIM + // @DisplayName: Maximum temperature of BEC circuits before shutdown or warning + // @Description: Maximum temperature of BEC circuits before shutdown or warning + // @Range: 0 100 + AP_GROUPINFO("_BEC_TEMP_LIM", 7, FSOPowerStack, bec_temperature_max, FSO_PAYLOAD_BEC_TEMPERATURE_MAX), + + // @Param: _FAN_1_MIN + // @DisplayName: Alarm threshold for minimum fan 1 tachometer in Hz + // @Description: Alarm threshold for minimum fan 1 tachometer in Hz + // @Range: 0 200 + AP_GROUPINFO("_FAN_1_MIN", 8, FSOPowerStack, fan_1_min_Hz, 0.0), + + // @Param: _FAN_2_MIN + // @DisplayName: Alarm threshold for minimum fan 2 tachometer in Hz + // @Description: Alarm threshold for minimum fan 2 tachometer in Hz + // @Range: 0 200 + AP_GROUPINFO("_FAN_2_MIN", 9, FSOPowerStack, fan_2_min_Hz, 0.0), + + // @Param: _FAN_3_MIN + // @DisplayName: Alarm threshold for minimum fan 2 tachometer in Hz + // @Description: Alarm threshold for minimum fan 2 tachometer in Hz + // @Range: 0 200 + AP_GROUPINFO("_FAN_3_MIN", 10, FSOPowerStack, fan_3_min_Hz, 0.0), + + // @Param: _FAN_4_MIN + // @DisplayName: Alarm threshold for minimum fan 2 tachometer in Hz + // @Description: Alarm threshold for minimum fan 2 tachometer in Hz + // @Range: 0 200 + AP_GROUPINFO("_FAN_4_MIN", 11, FSOPowerStack, fan_4_min_Hz, 0.0), AP_GROUPEND }; @@ -90,10 +145,16 @@ void FSOPowerStack::init() init_fan(FSO_FAN_TACH2_PIN, fans[1]); init_fan(FSO_FAN_TACH3_PIN, fans[2]); init_fan(FSO_FAN_TACH4_PIN, fans[3]); + + float sample_freq = 1.0 / FSO_LOOP_TIME_MS; + float over_current_tc = 1.0; + payload_HV_current_filter.set_cutoff_frequency(sample_freq, 1.0/over_current_tc); + payload_1_current_filter.set_cutoff_frequency(sample_freq, 1.0/over_current_tc); + payload_2_current_filter.set_cutoff_frequency(sample_freq, 1.0/over_current_tc); } /* - update fan RPMs + update fan frequency reading */ void FSOPowerStack::update_fans(void) { @@ -106,7 +167,7 @@ void FSOPowerStack::update_fans(void) for (auto &fan : fans) { if (fan.dt_count == 0) { - fan.rpm = 0; + fan.freq_hz = 0; continue; } void *irqstate = hal.scheduler->disable_interrupts_save(); @@ -114,13 +175,26 @@ void FSOPowerStack::update_fans(void) fan.dt_sum = 0; fan.dt_count = 0; hal.scheduler->restore_interrupts(irqstate); - fan.rpm = 60.0/(dt_avg*1.0e-6); + fan.freq_hz = 1.0/(dt_avg*1.0e-6); + } + + if (fans[0].freq_hz < fan_1_min_Hz){ + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Fan 1 failure"); + } + if (fans[1].freq_hz < fan_2_min_Hz){ + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Fan 2 failure"); + } + if (fans[2].freq_hz < fan_3_min_Hz){ + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Fan 3 failure"); + } + if (fans[3].freq_hz < fan_4_min_Hz){ + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Fan 4 failure"); } if (now_ms - last_fan_report_ms > 5000) { last_fan_report_ms = now_ms; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fans: %.1f %.1f %.1f %.1f", - fans[0].rpm, fans[1].rpm, fans[2].rpm, fans[3].rpm); + fans[0].freq_hz, fans[1].freq_hz, fans[2].freq_hz, fans[3].freq_hz); } } @@ -133,56 +207,56 @@ void FSOPowerStack::report_voltages(void) last_report_ms = now_ms; auto &batt = AP::battery(); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Voltages: 1:%.2f 2:%.2f 3:%.2f 4:%.2f 5:%.2f 6:%.2f 7:%.2f 8:%.2f 9:%.2f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Voltages: 1: %.2f, 2: %.2f, 3: %.2f, 4: %.2f, 5: %.2f, 6: %.2f, 7: %.2f, 8:%.2f, 9: %.2f", batt.voltage(0), batt.voltage(1), batt.voltage(2), batt.voltage(3), batt.voltage(4), batt.voltage(5), batt.voltage(6), batt.voltage(7), batt.voltage(8)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Current Limits: PHV: %.2f, P1: %.2f, P2: %.2f, IHC: %.2f, I1: %.2f, I2: %.2f", + payload_HV_current_filter.get(), payload_1_current_filter.get(), payload_2_current_filter.get(), + internal_HC_current_filter.get(), internal_1_current_filter.get(), internal_2_current_filter.get()); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Temperature Limits: P1: %.2f, P2: %.2f, IHC: %.2f, I1: %.2f, I2: %.2f", + payload_1_temp, payload_2_temp, internal_HC_temp, internal_1_temp, internal_2_temp); } void FSOPowerStack::update_switches() { uint32_t now_ms = AP_HAL::millis(); - bool main = hal.gpio->read(FSO_SWITCH_MAIN_PIN); - bool payload = hal.gpio->read(FSO_SWITCH_PAYLOAD_PIN); + bool switch_1_pressed = hal.gpio->read(FSO_SWITCH_MAIN_PIN); + bool switch_2_pressed = hal.gpio->read(FSO_SWITCH_PAYLOAD_PIN); - if (!main) { - last_main_on_ms = now_ms; - main_switch_released = true; + if (!switch_1_pressed) { + switch_1_press_time_ms = now_ms; + switch_1_switch_released = true; } - if (main_turn_on_state == Off){ - if (main_switch_released && (now_ms - last_main_on_ms > FSO_SWITCH_ON_TIME_MS)) { - main_turn_on_state = PreChargeStart; - if (payload_turn_on_state == Off){ - payload_turn_on_state = PreChargeStart; - } - main_switch_released = false; + if (switch_1_on == false){ + if (switch_1_switch_released && (now_ms - switch_1_press_time_ms > FSO_SWITCH_ON_TIME_MS)) { + switch_1_on = true; + switch_1_switch_released = false; } } else { - if (main_switch_released && (now_ms - last_main_on_ms > FSO_SWITCH_OFF_TIME_MS)) { - main_turn_on_state = ShutDown; - payload_turn_on_state = ShutDown; - main_switch_released = false; + if (switch_1_switch_released && (now_ms - switch_1_press_time_ms > FSO_SWITCH_OFF_TIME_MS)) { + switch_1_on = false; + switch_1_switch_released = false; } } - if (!payload) { - last_payload_on_ms = now_ms; - payload_switch_released = true; + if (!switch_2_pressed) { + switch_2_press_time_ms = now_ms; + switch_2_switch_released = true; } - if (payload_turn_on_state == Off){ - if (payload_switch_released && (now_ms - last_payload_on_ms > FSO_SWITCH_ON_TIME_MS)) { - payload_turn_on_state = PreChargeStart; - payload_switch_released = false; + if (switch_2_on == false){ + if (switch_2_switch_released && (now_ms - switch_2_press_time_ms > FSO_SWITCH_ON_TIME_MS)) { + switch_2_on = true; + switch_2_switch_released = false; } } else { - if (payload_switch_released && (now_ms - last_payload_on_ms > FSO_SWITCH_OFF_TIME_MS)) { - payload_turn_on_state = ShutDown; - payload_switch_released = false; + if (switch_2_switch_released && (now_ms - switch_2_press_time_ms > FSO_SWITCH_OFF_TIME_MS)) { + switch_2_on = false; + switch_2_switch_released = false; } } - } void FSOPowerStack::update_main_power() @@ -194,6 +268,9 @@ void FSOPowerStack::update_main_power() switch (main_turn_on_state) { case Off: + if (switch_1_on == true) { + main_turn_on_state = PreChargeStart; + } break; case PreChargeStart: @@ -227,6 +304,13 @@ void FSOPowerStack::update_main_power() break; case On: + if (switch_1_on == false) { + // Check if armed + // If armed override switch state + // switch_1_on == true; + // else + main_turn_on_state = ShutDown; + } break; case ShutDown: @@ -239,68 +323,170 @@ void FSOPowerStack::update_main_power() hal.gpio->write(FSO_BAT_1_EN_PIN, 0); hal.gpio->write(FSO_BAT_2_EN_PIN, 0); main_turn_on_state = Off; + + // Override swich state + switch_1_on = false; break; } } -void FSOPowerStack::update_payload_power() +void FSOPowerStack::update_payload_HV_power() { uint32_t now_ms = AP_HAL::millis(); auto &batt = AP::battery(); + float payload_HV_current; + if (batt.current_amps(payload_HV_current, 3)) { + payload_HV_current_filter.apply(payload_HV_current, 0.001 * FSO_LOOP_TIME_MS); + if (payload_HV_current_filter.get() > MIN(payload_HV_current_max, FSO_PAYLOAD_HV_CURRENT_MAX)) { + payload_HV_turn_on_state = ShutDown; + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Payload HV over current shutdown"); + } + } + // Payload Turn On State Machine - switch (payload_turn_on_state) { + switch (payload_HV_turn_on_state) { case Off: + if (switch_1_on == true) { + payload_HV_turn_on_state = PreChargeStart; + } break; case PreChargeStart: - if (fabsf(batt.voltage(0) - batt.voltage(1)) < FSO_BATT_VOLTS_DIFF_MAX) { - hal.gpio->write(FSO_LED_DEBUG_PIN, 1); - hal.gpio->write(FSO_PAYLOAD_PC_PIN, 1); - start_payload_precharge_ms = now_ms; - payload_turn_on_state = PreCharge; - break; - } else { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage"); - payload_turn_on_state = ShutDown; - } + // Turn on Payload BEC + hal.gpio->write(FSO_PAYLOAD_1_EN_PIN, 1); + hal.gpio->write(FSO_PAYLOAD_2_EN_PIN, 1); + + // Turn on Payload HV pre-charge + hal.gpio->write(FSO_PAYLOAD_HV_PC_PIN, 1); + start_payload_HV_precharge_ms = now_ms; + payload_HV_turn_on_state = PreCharge; break; case PreCharge: if (MAX(batt.voltage(0), batt.voltage(1)) - batt.voltage(2) < FSO_OUT_VOLTS_DIFF_MAX) { // Turn off Pre-Charge hal.gpio->write(FSO_LED_DEBUG_PIN, 0); - hal.gpio->write(FSO_PAYLOAD_PC_PIN, 0); + hal.gpio->write(FSO_PAYLOAD_HV_PC_PIN, 0); // Turn on Main Power and Led indicator hal.gpio->write(FSO_LED_PAYLOAD_PIN, 1); - hal.gpio->write(FSO_BAT_1_EN_PIN, 1); - hal.gpio->write(FSO_BAT_2_EN_PIN, 1); - payload_turn_on_state = On; + hal.gpio->write(FSO_PAYLOAD_HV_EN_PIN, 1); + payload_HV_turn_on_state = On; } - if (now_ms - start_payload_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) { - payload_turn_on_state = ShutDown; + if (now_ms - start_payload_HV_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) { + payload_HV_turn_on_state = ShutDown; } break; case On: + if (switch_1_on == false) { + payload_HV_turn_on_state = ShutDown; + } break; case ShutDown: + // Turn off Payload BEC + hal.gpio->write(FSO_PAYLOAD_1_EN_PIN, 0); + hal.gpio->write(FSO_PAYLOAD_2_EN_PIN, 0); + // Turn off Pre-Charge hal.gpio->write(FSO_LED_DEBUG_PIN, 0); - hal.gpio->write(FSO_PAYLOAD_PC_PIN, 0); + hal.gpio->write(FSO_PAYLOAD_HV_PC_PIN, 0); // Turn off Main Power and Led indicator hal.gpio->write(FSO_LED_PAYLOAD_PIN, 0); - hal.gpio->write(FSO_BAT_1_EN_PIN, 0); - hal.gpio->write(FSO_BAT_2_EN_PIN, 0); - payload_turn_on_state = Off; + hal.gpio->write(FSO_PAYLOAD_HV_EN_PIN, 0); + payload_HV_turn_on_state = Off; break; } } +void FSOPowerStack::update_payload_BEC() +{ + auto &batt = AP::battery(); + + float payload_1_current; + if (batt.current_amps(payload_1_current, 4)) { + payload_1_current_filter.apply(payload_1_current, 0.001 * FSO_LOOP_TIME_MS); + if (payload_1_current_filter.get() > MIN(payload_1_current_max, FSO_PAYLOAD_BEC_CURRENT_MAX)) { + hal.gpio->write(FSO_PAYLOAD_1_EN_PIN, 0); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Payload 1 over current shutdown"); + } + } + + float payload_2_current; + if (batt.current_amps(payload_2_current, 5)) { + payload_2_current_filter.apply(payload_2_current, 0.001 * FSO_LOOP_TIME_MS); + if (payload_2_current_filter.get() > MIN(payload_2_current_max, FSO_PAYLOAD_BEC_CURRENT_MAX)) { + hal.gpio->write(FSO_PAYLOAD_2_EN_PIN, 0); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Payload 2 over current shutdown"); + } + } + + if (batt.get_temperature(payload_1_temp, 4)) { + if (payload_1_temp > MIN(bec_temperature_max, FSO_PAYLOAD_BEC_TEMPERATURE_MAX)) { + hal.gpio->write(FSO_PAYLOAD_1_EN_PIN, 0); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Payload 1 over temperature shutdown"); + } + } + + if (batt.get_temperature(payload_2_temp, 5)) { + if (payload_2_temp > MIN(bec_temperature_max, FSO_PAYLOAD_BEC_TEMPERATURE_MAX)) { + hal.gpio->write(FSO_PAYLOAD_2_EN_PIN, 0); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Payload 2 over temperature shutdown"); + } + } +} + +void FSOPowerStack::update_internal_BEC() +{ + auto &batt = AP::battery(); + + float internal_HC_current; + if (batt.current_amps(internal_HC_current, 6)) { + internal_HC_current_filter.apply(internal_HC_current, 0.001 * FSO_LOOP_TIME_MS); + if (internal_HC_current_filter.get() > FSO_INTERNAL_BEC_HC_CURRENT_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC HC Over Current Fault"); + } + } + + float internal_1_current; + if (batt.current_amps(internal_1_current, 7)) { + internal_1_current_filter.apply(internal_1_current, 0.001 * FSO_LOOP_TIME_MS); + if (internal_1_current_filter.get() > FSO_INTERNAL_BEC_CURRENT_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC 1 Over Current Fault"); + } + } + + float internal_2_current; + if (batt.current_amps(internal_2_current, 8)) { + internal_2_current_filter.apply(internal_2_current, 0.001 * FSO_LOOP_TIME_MS); + if (internal_2_current_filter.get() > FSO_INTERNAL_BEC_CURRENT_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC 2 Over Current Fault"); + } + } + + if (batt.get_temperature(internal_HC_temp, 6)) { + if (internal_HC_temp > FSO_INTERNAL_BEC_TEMPERATURE_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC HC Over Temperature Fault"); + } + } + + if (batt.get_temperature(internal_1_temp, 7)) { + if (internal_1_temp > FSO_INTERNAL_BEC_TEMPERATURE_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC 1 Over Temperature Fault"); + } + } + + if (batt.get_temperature(internal_2_temp, 8)) { + if (internal_2_temp > FSO_INTERNAL_BEC_TEMPERATURE_MAX) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal BEC 2 Over Temperature Fault"); + } + } +} + /* update FSOPowerStack */ @@ -308,7 +494,7 @@ void FSOPowerStack::update() { uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_update_ms < 100) { + if (now_ms - last_update_ms < FSO_LOOP_TIME_MS) { // run at 10Hz return; } @@ -322,7 +508,11 @@ void FSOPowerStack::update() update_main_power(); - update_payload_power(); + update_payload_HV_power(); + + update_payload_BEC(); + + update_internal_BEC(); } #endif // HAL_PERIPH_ENABLE_FSO_POWER_STACK diff --git a/Tools/AP_Periph/FSOPowerStack.h b/Tools/AP_Periph/FSOPowerStack.h index c9eed85fa6b552..4666be9dc5e2ea 100644 --- a/Tools/AP_Periph/FSOPowerStack.h +++ b/Tools/AP_Periph/FSOPowerStack.h @@ -14,8 +14,16 @@ class FSOPowerStack { private: AP_Int32 options; - AP_Float p1_voltage; - AP_Float p2_voltage; + AP_Float payload_1_voltage; + AP_Float payload_2_voltage; + AP_Float payload_HV_current_max; + AP_Float payload_1_current_max; + AP_Float payload_2_current_max; + AP_Float bec_temperature_max; + AP_Float fan_1_min_Hz; + AP_Float fan_2_min_Hz; + AP_Float fan_3_min_Hz; + AP_Float fan_4_min_Hz; uint32_t last_update_ms; @@ -24,7 +32,7 @@ class FSOPowerStack { uint32_t last_pulse_us; uint32_t dt_sum; uint32_t dt_count; - float rpm; + float freq_hz; }; // States used during turn on @@ -47,22 +55,38 @@ class FSOPowerStack { void update_fans(); void update_switches(); void update_main_power(); - void update_payload_power(); + void update_payload_HV_power(); + void update_payload_BEC(); + void update_internal_BEC(); uint32_t last_report_ms; void report_voltages(); - bool main_switch_released; - bool payload_switch_released; - uint32_t start_main_precharge_ms; - uint32_t start_payload_precharge_ms; + bool switch_1_on; + bool switch_1_switch_released; + uint32_t switch_1_press_time_ms; + + bool switch_2_on; + bool switch_2_switch_released; + uint32_t switch_2_press_time_ms; - uint32_t last_main_on_ms; - uint32_t last_payload_on_ms; + uint32_t start_main_precharge_ms; + uint32_t start_payload_HV_precharge_ms; + LowPassFilterFloat payload_HV_current_filter; // Payload HV current input filter + LowPassFilterFloat payload_1_current_filter; // Payload BEC 1 current input filter + LowPassFilterFloat payload_2_current_filter; // Payload BEC 2 current input filter + float payload_1_temp; // Payload BEC 1 current input filter + float payload_2_temp; // Payload BEC 2 current input filter + LowPassFilterFloat internal_HC_current_filter; // Internal BEC HC current input filter + LowPassFilterFloat internal_1_current_filter; // Internal BEC 1 current input filter + LowPassFilterFloat internal_2_current_filter; // Internal BEC 2 current input filter + float internal_HC_temp; // Internal BEC HC current input filter + float internal_1_temp; // Internal BEC 1 current input filter + float internal_2_temp; // Internal BEC 2 current input filter TurnOnState main_turn_on_state = Off; - TurnOnState payload_turn_on_state = Off; + TurnOnState payload_HV_turn_on_state = Off; }; #endif // HAL_PERIPH_ENABLE_FSO_POWER_STACK diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FSOPowerStack/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FSOPowerStack/hwdef.dat index c2b070edd4d2ad..15d6643d6acce0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FSOPowerStack/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FSOPowerStack/hwdef.dat @@ -133,8 +133,10 @@ PC13 MAIN_PC OUTPUT PULLDOWN GPIO(74) LOW PC14 BAT_1_EN OUTPUT PULLDOWN GPIO(75) LOW PC15 BAT_2_EN OUTPUT PULLDOWN GPIO(76) LOW -define FSO_PAYLOAD_PC_PIN 70 -define FSO_PAYLOAD_EN_PIN 71 +define FSO_PAYLOAD_HV_PC_PIN 70 +define FSO_PAYLOAD_HV_EN_PIN 71 +define FSO_PAYLOAD_1_EN_PIN 72 +define FSO_PAYLOAD_2_EN_PIN 73 define FSO_MAIN_PC_PIN 74 define FSO_BAT_1_EN_PIN 75 define FSO_BAT_2_EN_PIN 76