Skip to content

Commit

Permalink
refactor: Clean up LV Controller
Browse files Browse the repository at this point in the history 
Remove some old abstractions like Indicator and StateBroadcaster to simplify the code. Move state broadcasts out of loops to prevent message floods. Update IsContactors functions to be more functional on the optional datatype to highlight how they may have empty data.
  • Loading branch information
@BlakeFreer
BlakeFreer committed Dec 30, 2024
1 parent d3671e0 commit 9f32f0b
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 153 deletions.
74 changes: 11 additions & 63 deletions firmware/projects/LVController/inc/app.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,40 +3,16 @@

#include <cmath>

#include "../generated/can/veh_bus.hpp"
#include "../generated/can/veh_messages.hpp"
#include "shared/os/mutex.hpp"
#include "shared/periph/gpio.hpp"
#include "shared/periph/pwm.hpp"
#include "shared/util/mappers/clamper.hpp"
#include "shared/util/mappers/mapper.hpp"

class StateBroadcaster {
public:
StateBroadcaster(generated::can::VehBus& can_bus) : can_bus_(can_bus) {}

void UpdateState(generated::can::LvControllerState state) {
generated::can::TxLvControllerStatus lv_status{
.lv_controller_state = static_cast<uint8_t>(state)};

can_bus_.Send(lv_status);
}

private:
generated::can::VehBus& can_bus_;
};

/**
* @brief A Subsystem which can be enabled / disabled.
*/
/// @brief A Subsystem which can be enabled / disabled.
class Subsystem {
public:
/**
* @brief Construct a new Subsystem object
*
* @param enable_output Digital Output which enables the subsystem.
* enabled).
*/
// Use shared::periph::InverseDigitalOutput if the subsystem is enabled by
// a low signal.
Subsystem(shared::periph::DigitalOutput& enable_output)
: enable_output_(enable_output) {}

Expand All @@ -52,26 +28,6 @@ class Subsystem {
shared::periph::DigitalOutput& enable_output_;
};

class Indicator {
public:
Indicator(shared::periph::DigitalOutput& output) : output_(output) {}

inline void TurnOn() {
output_.SetHigh();
}

inline void TurnOff() {
output_.SetLow();
}

inline void SetState(bool value) {
output_.Set(value);
}

private:
shared::periph::DigitalOutput& output_;
};

class Fan : public Subsystem {
public:
Fan(shared::periph::DigitalOutput& enable_output,
Expand All @@ -96,24 +52,16 @@ class Fan : public Subsystem {
duty_per_second_ = max_duty_per_second;
}

/**
* @brief Set the Power Now.
* @warning THIS IS DANGEROUS as large jumps can caused significant current
* rushes. Prefer SetTargetPower() and Update()
*
* @param power
*/
/// @brief Set the Power Now.
/// @warning THIS IS DANGEROUS as large jumps can caused significant current
/// rushes. Prefer SetTargetPower() and Update()
void Dangerous_SetPowerNow(float power) {
SetPower(power);
}

/**
* @brief Update the duty cycle towards the target duty cycle. The argument
* is the elapsed time between calls which is required to adjust the duty
* cycle by the rate specified in SetTargetPower.
*
* @param interval_sec
*/
/// @brief Update the duty cycle towards the target duty cycle. The argument
/// is the elapsed time between calls which is required to adjust the duty
/// cycle by the rate specified in SetTargetPower.
void Update(float interval_sec) const {
// max_duty_step must be greater than any platform's PWM duty cycle
// resolution
Expand Down Expand Up @@ -154,11 +102,11 @@ class DCDC : public Subsystem {

bool CheckValid() {
bool is_valid = valid_input_.Read();
led_.SetState(is_valid);
led_.Set(is_valid);
return is_valid;
}

private:
shared::periph::DigitalInput& valid_input_;
Indicator led_;
shared::periph::DigitalOutput& led_;
};
157 changes: 70 additions & 87 deletions firmware/projects/LVController/main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,18 @@
/// @date 2023-12-25

#include <cstdint>
#include <regex>

#include "bindings.hpp"
#include "generated/can/veh_bus.hpp"
#include "generated/can/veh_messages.hpp"
#include "inc/app.hpp"
#include "shared/periph/gpio.hpp"
#include "shared/periph/pwm.hpp"
#include "shared/util/mappers/identity.hpp"
#include "shared/util/mappers/mapper.hpp"

using namespace generated::can;

VehBus veh_can{bindings::veh_can_base};

StateBroadcaster state_tx{veh_can};

Subsystem tsal{bindings::tsal_en};
Subsystem raspberry_pi{bindings::raspberry_pi_en};
Subsystem front_controller{bindings::front_controller_en};
Expand All @@ -29,19 +24,11 @@ Subsystem motor_ctrl{bindings::motor_ctrl_en};
Subsystem imu_gps{bindings::imu_gps_en};
Subsystem shutdown_circuit{bindings::shutdown_circuit_en};
Subsystem inverter{bindings::inverter_switch_en};
Subsystem powertrain_pump{bindings::powertrain_pump_en};

auto dcdc_en_inverted =
shared::periph::InvertedDigitalOutput(bindings::dcdc_en);

DCDC dcdc{
dcdc_en_inverted,
bindings::dcdc_valid,
bindings::dcdc_led_en,
};
shared::periph::InvertedDigitalOutput dcdc_en_inverted(bindings::dcdc_en);

Subsystem powertrain_pump{
bindings::powertrain_pump_en,
};
DCDC dcdc{dcdc_en_inverted, bindings::dcdc_valid, bindings::dcdc_led_en};

auto powertrain_fan_power_to_duty = shared::util::IdentityMap<float>();
Fan powertrain_fan{
Expand All @@ -53,115 +40,112 @@ Fan powertrain_fan{
Subsystem all_subsystems[] = {
tsal, raspberry_pi, front_controller, speedgoat,
accumulator, motor_ctrl_precharge, motor_ctrl, imu_gps,
dcdc, powertrain_pump, powertrain_fan,
};
dcdc, powertrain_pump, powertrain_fan, shutdown_circuit};

void BroadcastState(LvControllerState state) {
veh_can.Send(TxLvControllerStatus{static_cast<uint8_t>(state)});
}

void DoPowerupSequence() {
tsal.Enable();

state_tx.UpdateState(LvControllerState::TsalEnabled);
BroadcastState(LvControllerState::TsalEnabled);

bindings::DelayMS(50);

// raspberry_pi.Enable();
state_tx.UpdateState(LvControllerState::RaspiEnabled);
raspberry_pi.Enable();
BroadcastState(LvControllerState::RaspiEnabled);

bindings::DelayMS(50);

front_controller.Enable();
state_tx.UpdateState(LvControllerState::FrontControllerEnabled);
BroadcastState(LvControllerState::FrontControllerEnabled);

bindings::DelayMS(100);

// speedgoat.Enable();
state_tx.UpdateState(LvControllerState::SpeedgoatEnabled);
speedgoat.Enable();
BroadcastState(LvControllerState::SpeedgoatEnabled);

bindings::DelayMS(100);

motor_ctrl_precharge.Enable();
state_tx.UpdateState(LvControllerState::MotorControllerPrechargeEnabled);
BroadcastState(LvControllerState::MotorControllerPrechargeEnabled);

bindings::DelayMS(2000);

motor_ctrl.Enable();
state_tx.UpdateState(LvControllerState::MotorControllerEnabled);
BroadcastState(LvControllerState::MotorControllerEnabled);

bindings::DelayMS(50);

motor_ctrl_precharge.Disable();
state_tx.UpdateState(LvControllerState::MotorControllerPrechargeDisabled);
BroadcastState(LvControllerState::MotorControllerPrechargeDisabled);

bindings::DelayMS(50);

// imu_gps.Enable();
state_tx.UpdateState(LvControllerState::ImuGpsEnabled);
imu_gps.Enable();
BroadcastState(LvControllerState::ImuGpsEnabled);
}

void DoPowertrainEnableSequence() {
void SweepFanBlocking() {
constexpr float kDutyInitial = 30.0f;
constexpr float kDutyFinal = 100.0f;
constexpr float kSweepPeriodSec = 3.0f;
constexpr float kMaxDutyRate =
(kDutyFinal - kDutyInitial) / kSweepPeriodSec;
constexpr float kUpdatePeriodSec = 0.01f;
constexpr uint32_t kUpdatePeriodMS = 10;

powertrain_fan.Dangerous_SetPowerNow(kDutyInitial);
powertrain_fan.SetTargetPower(kDutyFinal, kMaxDutyRate);

while (!powertrain_fan.IsAtTarget()) {
bindings::DelayMS(kUpdatePeriodMS);
powertrain_fan.Update(kUpdatePeriodMS / 1000.0f);
}
}

void DoPowertrainEnableSequence() {
dcdc.Enable();
state_tx.UpdateState(LvControllerState::DcdcEnabled);
BroadcastState(LvControllerState::DcdcEnabled);

do {
state_tx.UpdateState(LvControllerState::WaitingForDcdcValid);
bindings::DelayMS(50);
} while (!dcdc.CheckValid());
BroadcastState(LvControllerState::WaitingForDcdcValid);
while (!dcdc.CheckValid()) bindings::DelayMS(50);

bindings::DelayMS(50);

powertrain_pump.Enable();
state_tx.UpdateState(LvControllerState::DcdcLedEnabled);
BroadcastState(LvControllerState::DcdcLedEnabled);

bindings::DelayMS(100);

powertrain_fan.Enable();
state_tx.UpdateState(LvControllerState::PowertrainFanEnabled);
BroadcastState(LvControllerState::PowertrainFanEnabled);

bindings::DelayMS(50);

powertrain_fan.Dangerous_SetPowerNow(kDutyInitial);
powertrain_fan.SetTargetPower(kDutyFinal, kMaxDutyRate);

do {
state_tx.UpdateState(LvControllerState::PowertrainFanSweeping);
bindings::DelayMS(uint32_t(kUpdatePeriodSec * 1000));
powertrain_fan.Update(kUpdatePeriodSec);
} while (!powertrain_fan.IsAtTarget());
}

void DoPowertrainDisableSequence() {
powertrain_pump.Disable();
powertrain_fan.Disable();
BroadcastState(LvControllerState::PowertrainFanSweeping);
SweepFanBlocking(); // is it possible that this never returns?
}

bool IsContactorsOpen() {
auto contactor_states = veh_can.GetRxContactorStates();

if (contactor_states) {
return (contactor_states->PackPositive() == 0 &&
contactor_states->PackNegative() == 0 &&
contactor_states->PackPrecharge() == 0);
} else {
return false; // No data received yet
}
auto logic = [](auto contactor_states) {
return (contactor_states.PackPositive() == 0 &&
contactor_states.PackNegative() == 0 &&
contactor_states.PackPrecharge() == 0);
};

// If the contactor states are not available, return false.
return veh_can.GetRxContactorStates().transform(logic).value_or(false);
}

bool IsContactorsClosed() {
auto contactor_states = veh_can.GetRxContactorStates();

if (contactor_states) {
return (contactor_states->PackPositive() == 1 &&
contactor_states->PackNegative() == 1 &&
contactor_states->PackPrecharge() == 0);
} else {
return false; // No data received yet
}
auto logic = [](auto contactor_states) {
return (contactor_states.PackPositive() == 1 &&
contactor_states.PackNegative() == 1 &&
contactor_states.PackPrecharge() == 0);
};

// If the contactor states are not available, return false.
return veh_can.GetRxContactorStates().transform(logic).value_or(false);
}

void DoInverterSwitchCheck() {
Expand All @@ -177,44 +161,43 @@ void DoInverterSwitchCheck() {
int main(void) {
bindings::Initialize();

state_tx.UpdateState(LvControllerState::Startup);
BroadcastState(LvControllerState::Startup);

// Ensure all subsystems are disabled to start.
for (auto sys : all_subsystems) {
for (auto& sys : all_subsystems) {
sys.Disable();
}

// Powerup sequence
DoPowerupSequence();

while (true) continue; // stop after fc enable debug

do {
state_tx.UpdateState(LvControllerState::WaitingForOpenContactors);
BroadcastState(LvControllerState::WaitingForOpenContactors);
while (!IsContactorsOpen()) {
bindings::DelayMS(50);
} while (!IsContactorsOpen());
}

shutdown_circuit.Enable();

state_tx.UpdateState(LvControllerState::ShutdownCircuitEnabled);
BroadcastState(LvControllerState::ShutdownCircuitEnabled);

while (true) {
do {
state_tx.UpdateState(LvControllerState::WaitingForClosedContactors);
BroadcastState(LvControllerState::WaitingForClosedContactors);
while (!IsContactorsClosed()) {
bindings::DelayMS(50);
} while (!IsContactorsClosed());
}

DoPowertrainEnableSequence();
BroadcastState(LvControllerState::SequenceComplete);

do {
state_tx.UpdateState(LvControllerState::SequenceComplete);
while (dcdc.CheckValid()) {
DoInverterSwitchCheck();
} while (dcdc.CheckValid());
bindings::DelayMS(50);
}

state_tx.UpdateState(LvControllerState::LostDcdcValid);
BroadcastState(LvControllerState::LostDcdcValid);

inverter.Disable();
DoPowertrainDisableSequence();
powertrain_pump.Disable();
powertrain_fan.Disable();
}

return 0;
Expand Down
Loading

0 comments on commit 9f32f0b

Please sign in to comment.