Skip to content

Instantly share code, notes, and snippets.

@kingoflolz

kingoflolz/batt_tester.cpp

Created September 19, 2019 19:37
Show Gist options
  • Save kingoflolz/e68667319ce7effb66ba652568d221d0 to your computer and use it in GitHub Desktop.
Save kingoflolz/e68667319ce7effb66ba652568d221d0 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
batt_tester.cpp
#include <cmath>
#include <cmath>
/**
Battery Tester 3001
*/
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include "Utils.hpp"
#include "Adc.hpp"
#include "AnalogIn.hpp"
#include "Dac.hpp"
#include "DigitalOut.hpp"
#include "FreeRTOSConfig.h"
//#include "IndependentWatchdog.hpp"
#include "Rtc.hpp"
#include "Uart.hpp"
#include "CheckLimits.hpp"
#include <string>
//#include "Timer.hpp"
//#include "Debounce.hpp"
#include "MinMaxAvg.hpp"
#include "PwmOut.hpp"
/* MCU Pinouts ---------------------------------------------------------------*
| A0 B0 | PWR_SUPPLY_SENSE
BOARD_TEMP | A1 B1 |
MUX_TEMP_OUT | A2 B2 | CS
CURRENT_ADC | A3 B3 | DEBUG_SWO
DSCHG_CONTROL_DAC | A4 B4 | SYNC_ERROR
CHG_CONTROL_DAC | A5 B5 |
BATTERY_P_SENSE | A6 B6 | MUX3
BATTERY_N_SENSE | A7 B7 | MUX2
MCO | A8 B8 | MUX1
| A9 B9 | SD1_CD
CAN_GP_FLT | A10 B10 | ENABLE2
CAN_GP_RX | A11 B11 | ENABLE
CAN_GP_TX | A12 B12 |
DEBUG_SWDIO | A13 B13 | SCLK
DEBUG_SWCLK | A14 B14 | MISO
| A15 B15 | MOSI
////////////////////
| C0 D0 |
| C1 D1 |
| C2 D2 | SD1_CMD
| C3 D3 |
CHARGE_P_SENSE | C4 D4 | SD_READY1
CHARGE_N_SENSE | C5 D5 | CS_ADC
| C6 D6 |
| C7 D7 |
SD1_DAT0 | C8 D8 |
SD1_DAT1 | C9 D9 |
SD1_DAT2 | C10 D10 |
SD1_DAT3 | C11 D11 |
SD1_CLK | C12 D12 | SCL
| C13 D13 | SDA
| C14 D14 |
| C14 D14 |
////////////////////
UART8_RX | PE0 |
UART8_TX | PE1 |
TRACE_CLK | PE2 |
TRACE_D0 | PE3 |
TRACE_D1 | PE4 |
TRACE_D2 | PE5 |
TRACE_D3 | PE6 |
GPIO1 | PE7 |
GPIO2 | PE8 |
GPIO_5V_PG | PE9 |
| PE10 |
FRAM_CS | PE11 |
FRAM_SCK | PE12 |
FRAM_MISO | PE13 |
FRAM_MOSI | PE14 |
| PE15 |
* NOTES:
* Battery Current = (CURRENT_ADC - 2.5) / 0.02
* Battery Voltage = (BATTERY_P_SENSE - BATTERY_N_SENSE)
* Charger Voltage = (CHARGER_P_SENSE - CHARGER_N_SENSE)
* The Isolation Relay for Discharging is ENABLE and for Charging is ENABLE2
*----------------------------------------------------------------------------*/
#include <DigitalOut.hpp>
#include <task.h>
#include "Uart.hpp"
#include <cmsis_os.h>
#include "Rtc.hpp"
#include <IndependentWatchdog.hpp>
#include <ControllerUtils.hpp>
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wmissing-noreturn"
using namespace per::embedded::peripheral;
namespace per::embedded {
using namespace peripheral;
static void HandleWatchdogTimeout(uint32_t kickedThreads, bool startup);
static void CommandThread(void const *argument);
static void ControlThread(void const *argument);
osThreadId CommandThreadHandle;
osThreadId ControlThreadHandle;
Rtc rtc(true);
IndependentWatchdog<2> watchdog(20, 2000, cb::Callback2<void, uint32_t, bool>(HandleWatchdogTimeout));
constexpr uint8_t RTC_WATCHDOG_REGISTER = 0;
constexpr uint8_t RTC_WATCHDOG_STARTUP_REGISTER = 1;
static void HandleWatchdogTimeout(uint32_t kickedThreads, bool startup) {
rtc.WriteRegister(RTC_WATCHDOG_REGISTER, ~kickedThreads);
rtc.WriteRegister(RTC_WATCHDOG_STARTUP_REGISTER, startup);
Breakpoint();
}
/**
* \brief The entrypoint. You need to change it only if you are adding new threads or for debug purposes.
*/
int PlaygroundMain() {
CommandThreadHandle = CreateThread("Command", CommandThread, osPriorityNormal, 1024);
ControlThreadHandle = CreateThread("Control", ControlThread, osPriorityHigh, 1024);
watchdog.RegisterThreads(std::array<osThreadId, 2>{
CommandThreadHandle,
ControlThreadHandle
});
osKernelStart();
while (true)
Breakpoint();
}
enum State {
Idle = 0,
Discharging,
Charging
};
enum Error {
None,
Overcharge,
Overdischarge,
Overcurrent,
Overtemp,
CurrentRange,
InvalidCommand,
Timeout
};
State state = State::Idle;
Error error = Error::None;
float chargePower = 0;
float chargeMaxVoltage = 4.2;
float chargeCurrentLimit = 0;
float chargeIntegrator = 0;
float dischargePower = 0;
float dischargeMinVoltage = 2.7;
float dischargeCurrentLimit = 1;
float voltageHO, voltageLO;
PIDController currentController = PIDController(0, 0, 0, -0.1f, 0.9f, 0);
float selectedCurrent;
float battVoltage;
float cellTempreture;
bool processCommand(char *command) {
std::string commandString(&command[2]);
std::string::size_type bytesProcessed = 0;
switch (command[0]) {
case 'p':
{
//change pid values
float pValue = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
float iValue = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
float dValue = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
currentController.UpdateConstants(pValue, iValue, dValue, 0);
return commandString == std::string("\n");
}
case 'd':
{
//change discharge current and hard limit
dischargePower = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
dischargeCurrentLimit = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
dischargeMinVoltage = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
return commandString == std::string("\n");
}
case 'c':
{
//change charge current and hard limit
chargePower = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
chargeCurrentLimit = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
chargeMaxVoltage = std::stof (commandString,&bytesProcessed);
commandString = commandString.substr(bytesProcessed);
return commandString == std::string("\n");
}
case 's':
{
//change state
state = static_cast<State>(std::stoi(commandString, &bytesProcessed));
commandString = commandString.substr(bytesProcessed);
error = None;
return true;
}
case 'n':
{
// nop
return true;
}
default:
// unknown command
return false;
}
}
Timer tim9(9, Timer::Clock::Internal, 1, 2160);
PwmOut chargeCtrl(tim9, PE5);
PwmOut dischargeCtrl(tim9, PE6);
void ControlThread(void const *) {
TickType_t lastTime = xTaskGetTickCount();
const float ANALOG_REF = 3.3;
AnalogIn ext5v(PF3, nullptr, ADC_SAMPLETIME_480CYCLES);
AnalogIn currentH(PA3, nullptr, ADC_SAMPLETIME_480CYCLES);
AnalogIn currentL(PC0, nullptr, ADC_SAMPLETIME_480CYCLES);
//AnalogIn voltageCharger(PA0);
watchdog.Startup();
AnalogIn voltageL(PB1, nullptr, ADC_SAMPLETIME_480CYCLES);
AnalogIn voltageH(PF4, nullptr, ADC_SAMPLETIME_480CYCLES);
AnalogIn cellTemp(PC2, nullptr, ADC_SAMPLETIME_480CYCLES);
DigitalOut AIR(PF13);
LowPassFilter currentLPF;
LowPassFilter voltageLPF;
currentLPF.UpdateCoefficients(0.001f, .2f);
voltageLPF.UpdateCoefficients(0.001f, .2f);
while (true) {
float ext5vVoltage = 2 * ext5v.Read() * ANALOG_REF;
const float currLraw = 2 * currentL.Read() * ANALOG_REF;
const float currHraw = 2 * currentH.Read() * ANALOG_REF;
voltageHO = voltageH.Read();
voltageLO = voltageL.Read();
battVoltage = voltageLPF.Update(2 * (voltageH.Read()) * ANALOG_REF);
cellTempreture = 2 * cellTemp.Read() * ANALOG_REF / 0.01f - 273.15f;
if (currLraw > ext5vVoltage - 0.15f || currLraw < 0.15f){
state = State::Idle;
error = CurrentRange;
}
if (currHraw > ext5vVoltage - 0.15f || currHraw < 0.15f){
state = State::Idle;
error = CurrentRange;
}
float currL = (currLraw - ext5vVoltage/2) / 0.040f - 0.38f; // hardcoded calibration constants
float currH = (currHraw - ext5vVoltage/2) / 0.0052f - 57.1f;
selectedCurrent = currentLPF.Update((std::abs(currL) <= 45.0) ? currL : currH);
float measuredPower = selectedCurrent * battVoltage;
float multiplier = 0;
if (state == State::Charging) {
multiplier = -1;
} else if (state == State::Discharging) {
multiplier = 1;
}
float requestPower = 0;
if (state == State::Charging) {
requestPower = chargePower;
} else if (state == State::Discharging) {
requestPower = dischargePower;
}
float output = 0.1f + currentController.Command(0.001f, requestPower, std::fabs(measuredPower * multiplier));
if (state == State::Charging) {
if (std::fabs(selectedCurrent) > chargeCurrentLimit) {
state = State::Idle;
error = Overcurrent;
}
if (battVoltage > chargeMaxVoltage) {
state = State::Idle;
error = Overcharge;
}
dischargeCtrl.SetDutyCycle(0);
chargeCtrl.SetDutyCycle(output);
} else if (state == State::Discharging) {
if (std::fabs(selectedCurrent) > dischargeCurrentLimit) {
state = State::Idle;
error = Overcurrent;
}
if (battVoltage < dischargeMinVoltage) {
state = State::Idle;
error = Overdischarge;
}
dischargeCtrl.SetDutyCycle(output);
chargeCtrl.SetDutyCycle(0);
}
AIR.Set(state != State::Idle);
if (state == State::Idle) {
currentController.Reset();
dischargeCtrl.SetDutyCycle(0);
chargeCtrl.SetDutyCycle(0);
}
watchdog.Kick();
vTaskDelayUntil(&lastTime, 1);
}
}
void CommandThread(void const *) {
TickType_t lastTime = xTaskGetTickCount();
char commandBuffer[150] = {0};
uint commandBufferIdx = 0;
Uart interface(PD9, PD8, 1000, 1000, 912600);
interface.PrintF(100, "reset\n");
watchdog.Startup();
uint commandTimeout = 0;
while (true) {
bool can_read = true;
while (can_read && commandBufferIdx < 149) {
can_read = interface.TryReceive(reinterpret_cast<uint8_t *>(&commandBuffer[commandBufferIdx]), 1) > 0;
if (can_read && commandBuffer[commandBufferIdx] == '\n') {
commandBuffer[commandBufferIdx+1] = '\0';
if (processCommand(commandBuffer)) {
commandTimeout = 0;
interface.PrintF(100, "ok\n");
interface.PrintF(100, "i %f\n", selectedCurrent);
interface.PrintF(100, "v %f\n", battVoltage);
interface.PrintF(100, "t %f\n", cellTempreture);
interface.PrintF(100, "h %f\n", voltageHO);
interface.PrintF(100, "l %f\n", voltageLO);
interface.PrintF(100, "s %d\n", state);
interface.PrintF(100, "e %d\n", error);
interface.PrintF(100, "d %d\n", dischargeCtrl.GetCompareValue());
interface.PrintF(100, "c %d\n", chargeCtrl.GetCompareValue());
} else {
interface.PrintF(100, "fail\n");
// invalid command
error = InvalidCommand;
state = State::Idle;
}
commandBufferIdx = 0;
} else {
commandBufferIdx += can_read;
}
}
if (commandBufferIdx == 148) {
// overflow
state = State::Idle;
commandBufferIdx = 0;
}
commandTimeout++;
if (commandTimeout > 100) {
// timeout
error = Timeout;
state = State::Idle;
}
watchdog.Kick();
vTaskDelayUntil(&lastTime, 5);
}
}
}
int main() {
return per::embedded::PlaygroundMain();
}
#pragma clang diagnostic pop
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment