Skip to content

Commit

Permalink
AP_Perif: FSOPowerStack: Implement Main and Payload switching process
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Feb 3, 2024
1 parent 5560210 commit 9c6ef17
Show file tree
Hide file tree
Showing 2 changed files with 147 additions and 33 deletions.
161 changes: 134 additions & 27 deletions Tools/AP_Periph/FSOPowerStack.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,68 +143,169 @@ void FSOPowerStack::update_switches()
{
uint32_t now_ms = AP_HAL::millis();
bool main = hal.gpio->read(FSO_SWITCH_MAIN_PIN);
auto &batt = AP::battery();
bool payload = hal.gpio->read(FSO_SWITCH_PAYLOAD_PIN);

if (!main) {
last_main_on_ms = now_ms;
last_main_switch = false;
main_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;
}
} 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 (!payload) {
last_payload_on_ms = now_ms;
payload_switch_released = true;
}

// Alt Hold State Machine
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;
}
} 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;
}
}

}

void FSOPowerStack::update_main_power()
{
uint32_t now_ms = AP_HAL::millis();
auto &batt = AP::battery();

// Main Turn On State Machine
switch (main_turn_on_state) {

case Off:
if (!last_main_switch && (now_ms - last_main_on_ms > FSO_SWITCH_ON_TIME_MS)) {
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_MAIN_PC_PIN, 1);
start_main_precharge_ms = now_ms;
last_main_switch = true;
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_MAIN_PC_PIN, 1);
main_turn_on_state = PreCharge;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage");
}
main_turn_on_state = PreCharge;
break;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Unbalanced Battery Voltage");
main_turn_on_state = ShutDown;
}
break;

case PreCharge:
last_main_on_ms = now_ms;
if ((batt.voltage(0) - batt.voltage(8) < FSO_OUT_VOLTS_DIFF_MAX) && (batt.voltage(1) - batt.voltage(8) < FSO_OUT_VOLTS_DIFF_MAX)) {
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_MAIN_PC_PIN, 0);

// Turn on Main Power and Led indicator
hal.gpio->write(FSO_LED_MAIN_PIN, 1);
hal.gpio->write(FSO_BAT_1_EN_PIN, 1);
hal.gpio->write(FSO_BAT_2_EN_PIN, 1);
main_turn_on_state = On;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Capacitance: %.2f uF", 2.0*1e3*(now_ms - start_main_precharge_ms)/batt.voltage(8));
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turn On Time: %lu ms", now_ms - start_main_precharge_ms);
}
if (now_ms - start_main_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) {
hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
hal.gpio->write(FSO_MAIN_PC_PIN, 0);
main_turn_on_state = Off;
main_turn_on_state = ShutDown;
}
break;

case On:
if (!last_main_switch && (now_ms - last_main_on_ms > FSO_SWITCH_OFF_TIME_MS)) {
hal.gpio->write(FSO_LED_MAIN_PIN, 0);
hal.gpio->write(FSO_BAT_1_EN_PIN, 0);
hal.gpio->write(FSO_BAT_2_EN_PIN, 0);
main_turn_on_state = Off;
last_main_switch = true;
}
break;

case ShutDown:
// Turn off Pre-Charge
hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
hal.gpio->write(FSO_MAIN_PC_PIN, 0);

// Turn off Main Power and Led indicator
hal.gpio->write(FSO_LED_MAIN_PIN, 0);
hal.gpio->write(FSO_BAT_1_EN_PIN, 0);
hal.gpio->write(FSO_BAT_2_EN_PIN, 0);
main_turn_on_state = Off;
break;
}
}

void FSOPowerStack::update_payload_power()
{
uint32_t now_ms = AP_HAL::millis();
auto &batt = AP::battery();

// Payload Turn On State Machine
switch (payload_turn_on_state) {

case Off:
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;
}
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);

// 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;
}
if (now_ms - start_payload_precharge_ms > FSO_PRECHARGE_TIMEOUT_MS) {
payload_turn_on_state = ShutDown;
}
break;

case On:
break;

case ShutDown:
// Turn off Pre-Charge
hal.gpio->write(FSO_LED_DEBUG_PIN, 0);
hal.gpio->write(FSO_PAYLOAD_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;
break;
}
}

/*
update FSOPowerStack
*/
void FSOPowerStack::update()
{
update_switches();

uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_update_ms < 100) {
Expand All @@ -213,9 +314,15 @@ void FSOPowerStack::update()
}
last_update_ms = now_ms;

update_switches();

update_fans();

report_voltages();

update_main_power();

update_payload_power();
}

#endif // HAL_PERIPH_ENABLE_FSO_POWER_STACK
Expand Down
19 changes: 13 additions & 6 deletions Tools/AP_Periph/FSOPowerStack.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,11 +27,13 @@ class FSOPowerStack {
float rpm;
};

// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
// States used during turn on
enum TurnOnState {
Off,
PreChargeStart,
PreCharge,
On
On,
ShutDown
};

uint32_t last_fan_ms;
Expand All @@ -44,18 +46,23 @@ class FSOPowerStack {
void init_fan(uint8_t pin, FAN &fan);
void update_fans();
void update_switches();
void update_main_power();
void update_payload_power();

uint32_t last_report_ms;
void report_voltages();

bool last_main_switch;
bool last_payload_switch;
bool main_switch_released;
bool payload_switch_released;

uint32_t start_main_precharge_ms;
uint32_t start_payload_precharge_ms;

uint32_t last_main_on_ms;
uint32_t last_payload_on_ms;

TurnOnState main_turn_on_state;
TurnOnState payload_turn_on_state;
TurnOnState main_turn_on_state = Off;
TurnOnState payload_turn_on_state = Off;
};

#endif // HAL_PERIPH_ENABLE_FSO_POWER_STACK
Expand Down

0 comments on commit 9c6ef17

Please sign in to comment.