Skip to content

Commit

Permalink
reduced as many processes as possible, got to I2C being sent out and …
Browse files Browse the repository at this point in the history 
…got past init stage
  • Loading branch information
@Sabramz
Sabramz committed Jul 13, 2024
1 parent cbe162b commit 9c2a30a
Show file tree
Hide file tree
Showing 4 changed files with 84 additions and 83 deletions.
58 changes: 29 additions & 29 deletions Core/Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,12 +180,12 @@ int main(void)

/* Create Interfaces to Represent Relevant Hardware */
mpu_t *mpu = init_mpu(&hi2c1, &hadc3, &hadc1, GPIOC, GPIOB);
pdu_t *pdu = init_pdu(&hi2c2);
assert(pdu);
dti_t *mc = dti_init();
steeringio_t *wheel = steeringio_init();
init_can1(&hcan1);
bms_init();
//pdu_t *pdu = init_pdu(&hi2c2);
//assert(pdu);
// dti_t *mc = dti_init();
// steeringio_t *wheel = steeringio_init();
// init_can1(&hcan1);
// bms_init();

printf("\r\n\n\nInit Success...\r\n\n\n");

Expand All @@ -212,43 +212,43 @@ int main(void)

/* USER CODE BEGIN RTOS_THREADS */
/* Monitors */
lv_monitor_handle = osThreadNew(vLVMonitor, mpu, &lv_monitor_attributes);
assert(lv_monitor_handle);
// lv_monitor_handle = osThreadNew(vLVMonitor, mpu, &lv_monitor_attributes);
// assert(lv_monitor_handle);
// temp_monitor_handle = osThreadNew(vTempMonitor, mpu, &temp_monitor_attributes);
// assert(temp_monitor_handle);
imu_monitor_handle = osThreadNew(vIMUMonitor, mpu, &imu_monitor_attributes);
assert(imu_monitor_handle);
steeringio_buttons_monitor_handle = osThreadNew(vSteeringIOButtonsMonitor, wheel, &steeringio_buttons_monitor_attributes);
pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes);
assert(pedals_monitor_handle);
tsms_monitor_handle = osThreadNew(vTsmsMonitor, pdu, &tsms_monitor_attributes);
assert(tsms_monitor_handle);
fusing_monitor_handle = osThreadNew(vFusingMonitor, pdu, &fusing_monitor_attributes);
assert(fusing_monitor_handle);
// steeringio_buttons_monitor_handle = osThreadNew(vSteeringIOButtonsMonitor, wheel, &steeringio_buttons_monitor_attributes);
// pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes);
// assert(pedals_monitor_handle);
// tsms_monitor_handle = osThreadNew(vTsmsMonitor, pdu, &tsms_monitor_attributes);
// assert(tsms_monitor_handle);
// fusing_monitor_handle = osThreadNew(vFusingMonitor, pdu, &fusing_monitor_attributes);
// assert(fusing_monitor_handle);
// shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes);
// assert(shutdown_monitor_handle);

/* Messaging */
dti_router_handle = osThreadNew(vDTIRouter, mc, &dti_router_attributes);
assert(dti_router_handle);
can_dispatch_handle = osThreadNew(vCanDispatch, NULL, &can_dispatch_attributes);
assert(can_dispatch_handle);
bms_monitor_handle = osThreadNew(vBMSCANMonitor, NULL, &bms_monitor_attributes);
assert(bms_monitor_handle);
// dti_router_handle = osThreadNew(vDTIRouter, mc, &dti_router_attributes);
// assert(dti_router_handle);
// can_dispatch_handle = osThreadNew(vCanDispatch, NULL, &can_dispatch_attributes);
// assert(can_dispatch_handle);
// bms_monitor_handle = osThreadNew(vBMSCANMonitor, NULL, &bms_monitor_attributes);
// assert(bms_monitor_handle);
serial_monitor_handle = osThreadNew(vSerialMonitor, NULL, &serial_monitor_attributes);
assert(serial_monitor_handle);
nero_monitor_handle = osThreadNew(vNeroMonitor, NULL, &nero_monitor_attributes);
assert(nero_monitor_handle);
// nero_monitor_handle = osThreadNew(vNeroMonitor, NULL, &nero_monitor_attributes);
// assert(nero_monitor_handle);

/* Control Logic */
torque_calc_handle = osThreadNew(vCalcTorque, mc, &torque_calc_attributes);
assert(torque_calc_handle);
// torque_calc_handle = osThreadNew(vCalcTorque, mc, &torque_calc_attributes);
// assert(torque_calc_handle);
fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes);
assert(fault_handle);
sm_director_handle = osThreadNew(vStateMachineDirector, pdu, &sm_director_attributes);
assert(sm_director_handle);
brakelight_monitor_handle = osThreadNew(vBrakelightMonitor, pdu, &brakelight_monitor_attributes);;
assert(brakelight_monitor_handle);
// sm_director_handle = osThreadNew(vStateMachineDirector, pdu, &sm_director_attributes);
// assert(sm_director_handle);
// brakelight_monitor_handle = osThreadNew(vBrakelightMonitor, pdu, &brakelight_monitor_attributes);;
// assert(brakelight_monitor_handle);

/* USER CODE END RTOS_THREADS */

Expand Down
100 changes: 49 additions & 51 deletions Core/Src/monitor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -340,66 +340,64 @@ const osThreadAttr_t imu_monitor_attributes = {

void vIMUMonitor(void *pv_params)
{
static imu_data_t sensor_data;
fault_data_t fault_data = { .id = IMU_FAULT, .severity = DEFCON5 };
can_msg_t imu_accel_msg = { .id = CANID_IMU_ACCEL,
.len = 6,
.data = { 0 } };
can_msg_t imu_gyro_msg = { .id = CANID_IMU_GYRO,
.len = 6,
.data = { 0 } };
// static imu_data_t sensor_data;
// fault_data_t fault_data = { .id = IMU_FAULT, .severity = DEFCON5 };
// can_msg_t imu_accel_msg = { .id = CANID_IMU_ACCEL,
// .len = 6,
// .data = { 0 } };
// can_msg_t imu_gyro_msg = { .id = CANID_IMU_GYRO,
// .len = 6,
// .data = { 0 } };

mpu_t *mpu = (mpu_t *)pv_params;

for (;;) {
// serial_print("IMU Task\r\n");
printf("IMU Task\r\n");
/* Take measurement */
uint16_t accel_data[3] = { 0 };
uint16_t gyro_data[3] = { 0 };
if (read_accel(mpu, accel_data)) {
fault_data.diag = "Failed to get IMU acceleration";
queue_fault(&fault_data);
}

if (read_gyro(mpu, gyro_data)) {
fault_data.diag = "Failed to get IMU gyroscope";
queue_fault(&fault_data);
}

/* Run values through LPF of sample size */
sensor_data.accel_x =
(sensor_data.accel_x + accel_data[0]);
sensor_data.accel_y =
(sensor_data.accel_y + accel_data[1]);
sensor_data.accel_z =
(sensor_data.accel_z + accel_data[2]);
sensor_data.gyro_x =
(sensor_data.gyro_x + gyro_data[0]);
sensor_data.gyro_y =
(sensor_data.gyro_y + gyro_data[1]);
sensor_data.gyro_z =
(sensor_data.gyro_z + gyro_data[2]);

/* convert to big endian */
endian_swap(&sensor_data.accel_x, sizeof(sensor_data.accel_x));
endian_swap(&sensor_data.accel_y, sizeof(sensor_data.accel_y));
endian_swap(&sensor_data.accel_z, sizeof(sensor_data.accel_z));
endian_swap(&sensor_data.gyro_x, sizeof(sensor_data.gyro_x));
endian_swap(&sensor_data.gyro_y, sizeof(sensor_data.gyro_y));
endian_swap(&sensor_data.gyro_z, sizeof(sensor_data.gyro_z));

/* Send CAN message */
memcpy(imu_accel_msg.data, &sensor_data, imu_accel_msg.len);
if (queue_can_msg(imu_accel_msg) != HAL_OK) {
fault_data.diag = "Failed to send CAN message";
queue_fault(&fault_data);
HAL_StatusTypeDef res = read_accel(mpu, accel_data);
if (res != HAL_OK) {
printf("%d\n", res);
// fault_data.diag = "Failed to get IMU acceleration";
// queue_fault(&fault_data);
}

memcpy(imu_gyro_msg.data, &sensor_data, imu_gyro_msg.len);
if (queue_can_msg(imu_gyro_msg)) {
fault_data.diag = "Failed to send CAN message";
queue_fault(&fault_data);
}
printf("%d\n", res);

// if (read_gyro(mpu, gyro_data)) {
// fault_data.diag = "Failed to get IMU gyroscope";
// queue_fault(&fault_data);
// }

// /* Run values through LPF of sample size */
// sensor_data.accel_x = accel_data[0];
// sensor_data.accel_y = accel_data[1];
// sensor_data.accel_z = accel_data[2];
// sensor_data.gyro_x = gyro_data[0];
// sensor_data.gyro_y = gyro_data[1];
// sensor_data.gyro_z = gyro_data[2];

// /* convert to big endian */
// endian_swap(&sensor_data.accel_x, sizeof(sensor_data.accel_x));
// endian_swap(&sensor_data.accel_y, sizeof(sensor_data.accel_y));
// endian_swap(&sensor_data.accel_z, sizeof(sensor_data.accel_z));
// endian_swap(&sensor_data.gyro_x, sizeof(sensor_data.gyro_x));
// endian_swap(&sensor_data.gyro_y, sizeof(sensor_data.gyro_y));
// endian_swap(&sensor_data.gyro_z, sizeof(sensor_data.gyro_z));

// /* Send CAN message */
// memcpy(imu_accel_msg.data, &sensor_data, imu_accel_msg.len);
// if (queue_can_msg(imu_accel_msg) != HAL_OK) {
// fault_data.diag = "Failed to send CAN message";
// queue_fault(&fault_data);
// }

// memcpy(imu_gyro_msg.data, &sensor_data + imu_accel_msg.len, imu_gyro_msg.len);
// if (queue_can_msg(imu_gyro_msg)) {
// fault_data.diag = "Failed to send CAN message";
// queue_fault(&fault_data);
// }

/* Yield to other tasks */
osDelay(IMU_SAMPLE_DELAY);
Expand Down
7 changes: 5 additions & 2 deletions Core/Src/mpu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#define YLED_PIN GPIO_PIN_8
#define RLED_PIN GPIO_PIN_9
Expand Down Expand Up @@ -38,7 +39,7 @@ mpu_t *init_mpu(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *pedals_adc,
mpu->temp_sensor = malloc(sizeof(sht30_t));
assert(mpu->temp_sensor);
mpu->temp_sensor->i2c_handle = hi2c;
assert(!sht30_init(mpu->temp_sensor)); /* This is always connected */
//assert(!sht30_init(mpu->temp_sensor)); /* This is always connected */

assert(!HAL_ADC_Start_DMA(mpu->pedals_adc, mpu->pedal_dma_buf,
sizeof(mpu->pedal_dma_buf) /
Expand All @@ -50,7 +51,9 @@ mpu_t *init_mpu(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *pedals_adc,
/* Initialize the IMU */
mpu->imu = malloc(sizeof(lsm6dso_t));
assert(mpu->imu);
assert(!lsm6dso_init(mpu->imu, mpu->hi2c)); /* This is always connected */
HAL_StatusTypeDef res = lsm6dso_init(mpu->imu, mpu->hi2c);
printf(res + "\n");
// assert(!res); /* This is always connected */

/* Create Mutexes */
mpu->i2c_mutex = osMutexNew(&mpu_i2c_mutex_attr);
Expand Down
2 changes: 1 addition & 1 deletion Drivers/Embedded-Base
Submodule Embedded-Base updated 3 files
+1 −1 cangen/can-messages/dti.yaml
+45 −5 cangen/can-messages/mpu.yaml
+384 −0 cangen/can-messages/msb.yaml

0 comments on commit 9c2a30a

Please sign in to comment.