Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libraries/protobuf-definitions
26 changes: 26 additions & 0 deletions src/IO/ArduinoProxy.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,13 @@ void ArduinoProxy::handleArduinoMessage(const RocketryProto::ArduinoOut &arduino
dcMotorState.maxlimitswitch(), std::chrono::steady_clock::now()};
}
break;
case RocketryProto::ArduinoOut::kLoadCellState: {
std::lock_guard<std::mutex> lockGuard(stateMutex);

const auto &state = arduinoOut.loadcellstate();
loadCellState = {state.value(), std::chrono::steady_clock::now()};
}
break;
case RocketryProto::ArduinoOut::DATA_NOT_SET:
SPDLOG_LOGGER_WARN(logger, "Data field not set in Arduino message. ");
break;
Expand Down Expand Up @@ -211,4 +218,23 @@ DCMotorState ArduinoProxy::getDCMotorState(int forwardPin, int reversePin)
}
}

uint32_t ArduinoProxy::getLoadCellState()
{
std::lock_guard<std::mutex> lockGuard(stateMutex);

auto state = loadCellState;
auto now = std::chrono::steady_clock::now();

if (now - state.second >= pinStateTimeout)
{
loadCellState = {-1, std::chrono::steady_clock::now()};
SPDLOG_ERROR("Arduino stopped reporting load cell state");
return -1;
}
else
{
return state.first;
}
}

#endif
2 changes: 2 additions & 0 deletions src/IO/ArduinoProxy.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ class ArduinoProxy : IO
bool getDigitalState(int pin);
int getServoState(int pin);
DCMotorState getDCMotorState(int forwardPin, int reversePin);
uint32_t getLoadCellState();

ArduinoProxy(ArduinoProxy const &) = delete;
void operator=(ArduinoProxy const &) = delete;
Expand All @@ -33,6 +34,7 @@ class ArduinoProxy : IO
std::map<unsigned int, std::pair<bool, std::chrono::time_point<std::chrono::steady_clock>>> digitalStates;
std::map<unsigned int, std::pair<int, std::chrono::time_point<std::chrono::steady_clock>>> servoStates;
std::map<std::pair<unsigned int, unsigned int>, DCMotorState> dcMotorStates;
std::pair<uint32_t, std::chrono::time_point<std::chrono::steady_clock>> loadCellState;
std::mutex stateMutex;

int fd = 0;
Expand Down
3 changes: 3 additions & 0 deletions src/IO/SensorLogger.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -334,6 +334,9 @@ void SensorLogger::writeData(std::ofstream &fileStream, const StateData &current
{
fileStream << output.second << sep;
}


fileStream << currentState.gpioState.loadCellState << sep;
#endif

#if USE_SBG == 1
Expand Down
2 changes: 2 additions & 0 deletions src/IO/gpio/Gpio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,8 @@ GpioState Gpio::getCurrentState()
state.dcMotorStateMap.insert({i.first, i.second.getCurrentState()});
}

state.loadCellState = loadCell.getCurrentState();

return state;
}

Expand Down
2 changes: 2 additions & 0 deletions src/IO/gpio/Gpio.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include "IO/gpio/DCMotor.h"
#include "IO/gpio/DigitalOutput.h"
#include "IO/gpio/PwmOutput.h"
#include "IO/gpio/LoadCell.h"
#include "data/GpioData.h"
#include <condition_variable>
#include <fstream>
Expand Down Expand Up @@ -44,6 +45,7 @@ class Gpio : public IO
std::map<std::string, DigitalOutput> digitalOutputsMap;
std::map<std::string, PwmOutput> pwmOutputsMap;
std::map<std::string, DCMotorOutput> dcMotorOutputsMap;
LoadCell loadCell;

struct InitFlags
{
Expand Down
48 changes: 48 additions & 0 deletions src/IO/gpio/LoadCell.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
#include "config.h"

#if USE_GPIO == 1

#include "LoadCell.h"
#include <iostream>
#include <spdlog/spdlog.h>
#include <utility>
#include <wiringSerial.h>

LoadCell::LoadCell()
{
createThread = false;
logger = spdlog::default_logger();

SPDLOG_LOGGER_DEBUG(logger, "Created LoadCell");

#if USE_ARDUINO_PROXY == 1
arduinoProxy = ArduinoProxy::getInstance();
#endif
}

uint32_t LoadCell::getCurrentState()
{
#if USE_ARDUINO_PROXY == 1
try
{
return arduinoProxy->getLoadCellState();
}
catch (std::out_of_range &error)
{
return -1;
}
#else
return -1;
#endif
}

void LoadCell::run()
{
}

bool LoadCell::isInitialized()
{
return true;
}

#endif
35 changes: 35 additions & 0 deletions src/IO/gpio/LoadCell.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
#pragma once

#include "config.h"

#if USE_GPIO == 1

#include "IO/IO.h"
#include "Output.h"
#include "arduino/DCMotorState.h"

#if USE_ARDUINO_PROXY
#include "IO/ArduinoProxy.h"
#endif

#include <spdlog/logger.h>
#include <string>

class LoadCell : public IO
{
public:
LoadCell();

uint32_t getCurrentState();
void run() override;
bool isInitialized() override;

private:
std::shared_ptr<spdlog::logger> logger;

#if USE_ARDUINO_PROXY == 1
ArduinoProxy *arduinoProxy;
#endif
};

#endif
1 change: 1 addition & 0 deletions src/data/GpioData.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,4 +16,5 @@ struct GpioState
std::map<std::string, int> digitalStateMap;
std::map<std::string, int> pwmStateMap;
std::map<std::string, DCMotorState> dcMotorStateMap;
uint32_t loadCellState;
};
3 changes: 3 additions & 0 deletions src/data/StateData.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,9 @@ std::string StateData::convertToReducedString() const
data += std::to_string(output.second.maxLimitSwitch);
data += ",";
}

data += std::to_string(gpioState.loadCellState);
data += ",";
#endif

#if USE_SENSOR_SUITE == 1
Expand Down
20 changes: 16 additions & 4 deletions src/stateMachine/HotFire/HotFireGpioConfig.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,15 @@

#define USE_PWM_FILL 1

#define USE_PURGE 1

#define USE_HEATER 1

#define EVENT_ENABLE_MASK 0b1

#if USE_VENT
#define VENT_NAME "VENT"
#define VENT_PIN 8
#define VENT_PIN 13
#define VENT_OPEN 1
#define VENT_CLOSE 0
#define VENT_SAFE 1
Expand All @@ -32,8 +34,8 @@
#if USE_IGNITER
#define IGNITER_NAME "IGNITER"
#define IGNITER_PIN 12
#define IGNITER_ON 1
#define IGNITER_OFF 0
#define IGNITER_ON 0
#define IGNITER_OFF 1
#define IGNITER_EVENT_ENABLE_MASK 0b100
#endif

Expand All @@ -46,7 +48,7 @@
#define MAIN_POTENTIOMETER_PIN 0
#define MAIN_MOTOR_POWER 255
#define MAIN_OPEN 0
#define MAIN_IGNITION 400
#define MAIN_IGNITION_BURN 400
#define MAIN_CLOSE 1023
#define MAIN_EVENT_ENABLE_MASK 0b1000
#endif
Expand All @@ -71,6 +73,16 @@
#define FILL_SOFTPWM true
#endif

#if USE_PURGE
#define PURGE_NAME "PURGE"
#define PURGE_PIN 3
#define PURGE_OPEN 180
#define PURGE_CLOSE 0
#define PURGE_SAFE 0
#define PURGE_EVENT_ENABLE_MASK 0b1000000
#define PURGE_SOFTPWM true
#endif

#if USE_HEATER
#define HEATER_NAME "HEATER"
#define HEATER_PIN 7
Expand Down
Loading