Skip to content

Commit

Permalink
Added in embedded science code (#669)
Browse files Browse the repository at this point in the history 
* Added in embedded science code

* Applied black formatting

* Update science code in ROS

* Remove ghost comments

---------

Co-authored-by: tabiosg <[email protected]>
  • Loading branch information
@YnotCode @tabiosg
YnotCode and tabiosg authored Mar 29, 2024
1 parent a6d372f commit c96726e
Show file tree
Hide file tree
Showing 15 changed files with 276 additions and 316 deletions.
18 changes: 15 additions & 3 deletions src/esw/fw/science/.cproject
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.985754373" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1395189486" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G431CBUx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include | ../Middlewares/Third_Party/FreeRTOS/Source/include | ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 | ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F || || || USE_HAL_DRIVER | STM32G431xx || || Drivers | Core/Startup | Middlewares | Core || || || ${workspace_loc:/${ProjName}/STM32G431CBUX_FLASH.ld} || true || NonSecure || || secure_nsclib.o || || None || || || " valueType="string"/>
<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1321090" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="140" valueType="string"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.1878252718" name="Runtime library" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_standard_cpp" valueType="enumerated"/>
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.2091356620" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
<builder buildPath="${workspace_loc:/science}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.675342016" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1357413798" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
Expand Down Expand Up @@ -56,7 +57,7 @@
</tool>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.1689170643" name="MCU G++ Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler">
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.938044064" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1465919741" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.1465919741" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.optimization.level.value.o0" valueType="enumerated"/>
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths.1781137525" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.includepaths" useByScannerDiscovery="false" valueType="includePath">
<listOptionValue builtIn="false" value="../Core/Inc"/>
<listOptionValue builtIn="false" value="../Drivers/STM32G4xx_HAL_Driver/Inc"/>
Expand All @@ -72,7 +73,11 @@
<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
<listOptionValue builtIn="false" value="STM32G431xx"/>
</option>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.63798556" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.value.gnupp20" valueType="enumerated"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.63798556" name="Language standard" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard" useByScannerDiscovery="true" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.languagestandard.value.gnupp20" valueType="enumerated"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.noexceptions.1018730593" name="Disable handling exceptions (-fno-exceptions)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.noexceptions" useByScannerDiscovery="false" value="false" valueType="boolean"/>
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags.1185353284" name="Other flags" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags" useByScannerDiscovery="true" valueType="stringList">
<listOptionValue builtIn="false" value="-fexceptions"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.2137496482" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
</tool>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.558417596" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
Expand Down Expand Up @@ -233,5 +238,12 @@
<autodiscovery enabled="false" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Debug">
<resource resourceType="PROJECT" workspacePath="/science"/>
</configuration>
<configuration configurationName="Release">
<resource resourceType="PROJECT" workspacePath="/science"/>
</configuration>
</storageModule>
</cproject>
7 changes: 3 additions & 4 deletions src/esw/fw/science/Core/Inc/i2c_mux.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,16 +28,15 @@ namespace mrover {
public:
I2CMux() = default;

I2CMux(std::shared_ptr<SMBus<uint8_t, uint16_t>> i2c_bus, Pin reset_pin);
I2CMux(std::shared_ptr<SMBus<uint8_t, uint8_t>> i2c_bus, Pin reset_pin);

void set_channel(uint8_t channel);

private:
std::shared_ptr<SMBus<uint8_t, uint16_t>> m_i2c_bus;
std::shared_ptr<SMBus<uint8_t, uint8_t>> m_i2c_bus;
uint8_t current_channel = 0;
constexpr static std::uint16_t MUX_7b_ADDRESS = 0x70;
Pin m_reset_pin;
};

} // namespace mrover

} // namespace mrover
47 changes: 21 additions & 26 deletions src/esw/fw/science/Core/Inc/science.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,14 @@
#include "hardware_adc.hpp"
#include "hardware_i2c.hpp"


namespace mrover {

class Science {
private:

std::shared_ptr<SMBus<uint8_t, uint8_t>> m_i2c_bus;

FDCAN<InBoundScienceMessage> m_fdcan_bus;
std::array<Spectral, 3> m_spectral_sensors;
std::shared_ptr<ADCSensor> m_adc_sensor;
Expand Down Expand Up @@ -100,38 +103,30 @@ namespace mrover {
std::visit([&](auto const& command) { feed(command); }, message);
}

void poll_spectral_status() {
m_spectral_sensors.at(0).poll_status_reg();
}

void reboot_spectral() {
m_spectral_sensors.at(0).reboot();
void reboot_i2c() {
//m_spectral_sensors.at(0).reboot();
m_i2c_bus->reboot();
}

void update_and_send_spectral() {
SpectralData spectral_data;
for (int i = 0; i < 3; ++i) {
SpectralData spectral_data;
spectral_data.site = i;
m_spectral_sensors.at(i).update_channel_data();

spectral_data.spectrals.at(i).error =
spectral_data.error =
m_spectral_sensors.at(i).is_error();
if (spectral_data.error) {

// reboot_i2c(); // This causes a crash

}
for (int j = 0; j < 6; ++j) {
spectral_data.spectrals.at(i).data.at(j) =
spectral_data.data.at(j) =
m_spectral_sensors.at(i).get_channel_data(j);
}
// for (int j = 0; j < 6; ++j) {
// m_spectral_sensors.at(i).update_channel_data(j);
// spectral_data.spectrals.at(i).data.at(j) =
// m_spectral_sensors.at(i).get_channel_data(j);
// spectral_data.spectrals.at(i).error =
// m_spectral_sensors.at(i).is_error();
// }
m_fdcan_bus.broadcast(OutBoundScienceMessage{spectral_data});
}

// TODO - MUTEXS ARE BREAKING CODE!!!! IDK WHY - PLEASE FIX
// osMutexAcquire(m_can_tx_mutex, osWaitForever);
m_fdcan_bus.broadcast(OutBoundScienceMessage{spectral_data});
// osMutexRelease(m_can_tx_mutex);
}

void update_and_send_thermistor_and_auto_shutoff_if_applicable() {
Expand All @@ -145,9 +140,9 @@ namespace mrover {


/* send current and temperature over CAN */
osMutexAcquire(m_can_tx_mutex, osWaitForever);
// osMutexAcquire(m_can_tx_mutex, osWaitForever);
m_fdcan_bus.broadcast(OutBoundScienceMessage{thermistor_data});
osMutexRelease(m_can_tx_mutex);
// osMutexRelease(m_can_tx_mutex);
}

void update_and_send_heater() {
Expand All @@ -162,11 +157,11 @@ namespace mrover {
SET_BIT_AT_INDEX(heater_state_data.heater_state_info.on, i, m_heaters.at(i).get_state());
}

osMutexAcquire(m_can_tx_mutex, osWaitForever);
// osMutexAcquire(m_can_tx_mutex, osWaitForever);
m_fdcan_bus.broadcast(OutBoundScienceMessage{heater_state_data});
osMutexRelease(m_can_tx_mutex);
// osMutexRelease(m_can_tx_mutex);
}

};

} // namespace mrover
} // namespace mrover
35 changes: 24 additions & 11 deletions src/esw/fw/science/Core/Inc/spectral.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,39 +20,52 @@ namespace mrover {
public:
Spectral() = default;

Spectral(std::shared_ptr<SMBus<uint8_t, uint16_t>> i2c_bus, std::shared_ptr<I2CMux> i2c_mux, uint8_t i2c_mux_channel);
Spectral(std::shared_ptr<SMBus<uint8_t, uint8_t>> i2c_bus, std::shared_ptr<I2CMux> i2c_mux, uint8_t i2c_mux_channel);

void poll_status_reg();
enum I2C_OP{
READ,
WRITE
};
void poll_status_reg(I2C_OP rw);

void update_channel_data(); // updates all of the channels

uint16_t get_channel_data(uint8_t channel);
float get_channel_data(uint8_t channel);

void reboot();

bool is_error();

void init();

void virtual_write(uint8_t virtual_reg, uint8_t data);
uint8_t virtual_read(uint8_t virtual_reg);// -> std::optional<uint16_t>;

constexpr static std::uint16_t SPECTRAL_7b_ADDRESS = 0x49;
constexpr static std::uint8_t I2C_AS72XX_SLAVE_STATUS_REG = 0x00;
constexpr static std::uint8_t I2C_AS72XX_WRITE_REG = 0x01;
constexpr static std::uint8_t I2C_AS72XX_READ_REG = 0x02;
constexpr static std::uint8_t I2C_AS72XX_SLAVE_TX_VALID = 0x02;
constexpr static std::uint8_t I2C_AS72XX_SLAVE_RX_VALID = 0x01;
constexpr static std::uint8_t CONTROL_SETUP_REG = 0x04;
constexpr static std::uint8_t INT_TIME_REG = 0x05;

private:
bool m_error{};
bool m_initialized{};
bool m_error{false};
bool m_initialized{false};

std::shared_ptr<SMBus<uint8_t, uint16_t>> m_i2c_bus;
std::shared_ptr<SMBus<uint8_t, uint8_t>> m_i2c_bus;
std::shared_ptr<I2CMux> m_i2c_mux;
uint8_t m_i2c_mux_channel;
constexpr static std::uint8_t CHANNEL_DATA_LENGTH = 6;
std::array<uint16_t, CHANNEL_DATA_LENGTH> channel_data {};
std::array<float, CHANNEL_DATA_LENGTH> channel_data {};
// Sensor Raw Data Registers Start, 6 channels, 2 bytes each.
// See pg. 22 of datasheet for more info.
// constexpr static std::uint8_t CHANNEL_V_HIGH = 0x08;
// Sensor Calibrated Data Registers Start (p. 23 on datasheet)
constexpr static std::uint8_t CHANNEL_V_HIGH = 0x08;
constexpr static std::uint8_t CONTROL_SETUP_REG = 0x04;
constexpr static std::uint8_t INT_TIME_REG = 0x05;
constexpr static std::uint8_t CHANNEL_V_CAL = 0x14;

};

} // namespace mrover

} // namespace mrover
6 changes: 3 additions & 3 deletions src/esw/fw/science/Core/Src/diag_temp_sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,10 @@ namespace mrover {
}

void DiagTempSensor::update_science_temp() {
// Magic number used to calibrate thermistor temperature
float adc_cnt = m_adc_sensor->get_raw_channel_value(m_channel);
float measured_voltage = m_adc_sensor->get_raw_channel_value(m_channel) * 3.3f / 4096.0f;
float measured_resistance = ((m_adc_sensor->get_raw_channel_value(m_channel) * 3.3f / 4096.0f) * RESISTANCE_25)/(3.3f - (m_adc_sensor->get_raw_channel_value(m_channel) * 3.3f / 4096.0f)) / 10;
float measured_resistance = (measured_voltage * RESISTANCE_25)/(3.3f - measured_voltage);
m_temp = 1/(THRM_A + THRM_B*log(measured_resistance/RESISTANCE_25) + THRM_C*log((measured_resistance/RESISTANCE_25)*(measured_resistance/RESISTANCE_25)) + THRM_D*(((measured_resistance/RESISTANCE_25)*(measured_resistance/RESISTANCE_25)*(measured_resistance/RESISTANCE_25))));
m_temp -= 273.15;
}
Expand All @@ -41,5 +42,4 @@ namespace mrover {
return m_temp;
}

} // namespace mrover

} // namespace mrover
7 changes: 3 additions & 4 deletions src/esw/fw/science/Core/Src/heater.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@

namespace mrover {

constexpr static float MAX_HEATER_TEMP = 65.0f;
constexpr static int MAX_HEATER_WATCHDOG_TICK = 3000;
constexpr static float MAX_HEATER_TEMP = 50.0f;
constexpr static int MAX_HEATER_WATCHDOG_TICK = 1000;

Heater::Heater(DiagTempSensor const& diag_temp_sensor, Pin const& heater_pin)
: m_diag_temp_sensor(std::move(diag_temp_sensor)),
Expand Down Expand Up @@ -67,5 +67,4 @@ namespace mrover {
m_auto_shutoff_enabled = enable;
}

} // namespace mrover

} // namespace mrover
16 changes: 5 additions & 11 deletions src/esw/fw/science/Core/Src/i2c_mux.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,29 +2,23 @@
// Hardware PCA9546A
namespace mrover {

I2CMux::I2CMux(std::shared_ptr<SMBus<uint8_t, uint16_t>> i2c_bus, Pin reset_pin)
I2CMux::I2CMux(std::shared_ptr<SMBus<uint8_t, uint8_t>> i2c_bus, Pin reset_pin)
: m_i2c_bus(i2c_bus), m_reset_pin(reset_pin) {
reset_pin.write(GPIO_PIN_SET);
}

void I2CMux::set_channel(uint8_t channel) {
uint8_t go_to_channel = 1 << channel;
auto result = m_i2c_bus->blocking_transact(MUX_7b_ADDRESS, go_to_channel);
if(result){
if(result.has_value()){
current_channel = go_to_channel;
}
else{
m_reset_pin.write(GPIO_PIN_RESET);
osDelay(50);
m_reset_pin.write(GPIO_PIN_SET);
// Error handling
}

// Clear read and write semaphore so that tasks need to re-acquire them
if(osSemaphoreGetCount(spectral_read_status) > 0){
osSemaphoreAcquire(spectral_read_status, osWaitForever);
}

if(osSemaphoreGetCount(spectral_write_status) > 0){
osSemaphoreAcquire(spectral_write_status, osWaitForever);
}
}
} // namespace mrover

Expand Down
Loading

0 comments on commit c96726e

Please sign in to comment.