diff --git a/telemetry/firmware/queue.h b/telemetry/firmware/queue.h new file mode 100644 index 0000000..f0ee6ca --- /dev/null +++ b/telemetry/firmware/queue.h @@ -0,0 +1,106 @@ +/** +\file queue.h +\brief A generic FIFO queue. +*/ + +#ifndef QUEUE_H +#define QUEUE_H + +#include +#include + +/* +https://github.com/clnhlzmn/utils/tree/master/queue + +A very simple circular buffer. +Example: +QUEUE(test, int, count) +Creates: +struct queue_test {...}; +static inline void queue_test_init(struct queue_test *) {...} +static inline int queue_test_push(struct queue_test *, int *) {...} +static inline int queue_test_pop(struct queue_test *, int *) {...} +API: +queue_*_init initializes a queue +queue_*_push pushes an item onto the queue, returns 0 if successful, not 0 if fail +queue_*_pop pops an item from the queue, returns 0 if successful, not 0 if fail +queue_*_foreach takes a function pointer and pointer to some context and for each + element in the queue calls the function with a pointer to that element. If the + returns zero queue_*_foreach will continue processing the rest of the items, if + the function returns non zero then queue_*_foreach will not process any more items. +*/ + +/** +\brief Generates the queue api +\param name a name for the api with the given type and size +\param type the type of data to store in the queue +\param size the max number of data elements +*/ +#define QUEUE(name, type, size) \ +struct queue_##name { \ + type storage[size]; \ + /*index of the read head, initialy 0*/ \ + size_t read; \ + /*index of the write head, initialy 0*/ \ + size_t write; \ + /*number of items in the queue*/ \ + size_t count; \ +}; \ +static inline int queue_##name##_init(volatile struct queue_##name *self) { \ + if (!self) return -1; \ + self->read = 0; \ + self->write = 0; \ + self->count = 0; \ + return 0; \ +} \ +static inline int queue_##name##_push(volatile struct queue_##name *self, \ + const volatile type *item) { \ + if (!self || !item) return -1; \ + if (self->count < size) { \ + size_t next = (self->write + 1) % size; \ + self->storage[next] = *item; \ + self->write = next; \ + self->count++; \ + return 0; \ + } else { \ + return -1; \ + } \ +} \ +static inline int queue_##name##_peek(volatile struct queue_##name *self, \ + volatile type *item) { \ + if (!self || !item) return -1; \ + if (self->count > 0) { \ + *item = self->storage[next]; \ + return 0; \ + } else { \ + return -1; \ + } \ +} \ +static inline int queue_##name##_pop(volatile struct queue_##name *self, \ + volatile type *item) { \ + if (!self || !item) return -1; \ + if (self->count > 0) { \ + size_t next = (self->read + 1) % size; \ + *item = self->storage[next]; \ + self->read = next; \ + self->count--; \ + return 0; \ + } else { \ + return -1; \ + } \ +} \ +static inline size_t queue_##name##_count(const volatile struct queue_##name *self) { \ + if (!self) return 0; \ + return self->count; \ +} \ +static inline void queue_##name##_foreach(volatile struct queue_##name *self, \ + int (*fun)(volatile type *, volatile void *), \ + volatile void *ctx) { \ + if (!self) return; \ + if (fun == NULL) return; \ + for (size_t i = 0; i < self->count; ++i) { \ + if (fun(&self->storage[(self->read + i + 1) % size], ctx) != 0) break; \ + } \ +} + +#endif //QUEUE_H \ No newline at end of file diff --git a/telemetry/firmware/telemetry.c b/telemetry/firmware/telemetry.c new file mode 100644 index 0000000..c3b1982 --- /dev/null +++ b/telemetry/firmware/telemetry.c @@ -0,0 +1,362 @@ +/* + Header: + This is the functional code for the telemetry board + Function: Responsible for pulling safety & failure critical information from CANBus and passing it along to UART for telemetry RF module + : Interface with RF Module + : Interface with CANBus + + + confirm UART setup + confirm CAN setup + setup UART sending + + Author: + adi ramachandran + aramachandran@olin.edu + 6307969467 + + */ + + +/*----- Includes -----*/ +#include +#include +#include "can_api.h" +#include "queue.h" + +/*----- Macro Definitions -----*/ + + +/* Defining queue structures +- lengths are sort of arbitrary +*/ +#define UART_QUEUE_SIZE 512 // 512 bytes too long? + + +// gFlag positions +#define UART_SEND_READY 0 +#define UART_RECIEVED 1 + + +// CAN Mailboxes +#define THROTTLE_CMD_MOB 0 +#define THROTTLE_BRD_MOB 1 +#define BRAKE_MOB 2 +#define AIR_CONTROL_SENSE_MOB 3 +#define BMS_CORE_MOB 4 +#define CELL_VOLT_TEMP_MOB 5 + +#define CAN_MSK_6 0b11000000 // custom mask for cell temps & voltages +// handle another 15 mailboxes lol + + +// UART + +// values within LINCR register +#define ENABLE_LIN_AND_UART LENA // can i leave this as LENA or does it have to be bit 3 (page 190) +#define ENABLE_UART LCMD2 +#define ENABLE_RX LMCD1 +#define ENABLE_TX LCMD0 + +#define UART_DATA LINDAT // lin data register +#define UART_BAUD 9600 + + +// shutdown_error_flag codes within error var + +#define BSPD_FAIL 0 +#define INERTIA_SWITCH_FAIL 1 +#define BOTS_FAIL 2 +#define COCKPIT_ESTOP_FAIL 3 +#define HV_DISCONNECT_FAIL 4 +#define HVD_CONN_FAIL 5 +#define PACK_CONN_FAIL 6 +#define BMS_FAIL 7 +#define IMD_FAIL 8 +#define TSMS_FAIL 9 + + +/*----- Global Variables -----*/ + +volatile uint8_t global_flag = 0x00; // global flag + +volatile uint16_t shutdown_error_flag = 0x00; // system failure error code flag - default 0x00. + +volatile uint8_t can_recv_msg[8] = {0}; // CAN msg recieved + + +QUEUE(UART, uint8_t, UART_QUEUE_SIZE); +voltatile struct queue_UART UART_queue; // creating a UART_queue of type queue_UART + + + + + +/*----- Interrupt(s) -----*/ + + +ISR(CAN_INT_vect){ + + // previuosly checked for MOB full by setting CANPAGE and checking RXOK bit of CANSTMOB, with: + // CANPAGE = (THROTTLE_CMD_MOB<>3 + CANIDT3<<5; // pg 169 + + // transmit ID, then 8 bytes or 4 bytes + queue_UART_push(UART_queue, &(msg_ID)); + queue_UART_push(UART_queue, &(can_recv_msg[0])); + queue_UART_push(UART_queue, &(can_recv_msg[1])); + queue_UART_push(UART_queue, &(can_recv_msg[2])); + queue_UART_push(UART_queue, &(can_recv_msg[3])); + queue_UART_push(UART_queue, &(can_recv_msg[4])); + queue_UART_push(UART_queue, &(can_recv_msg[5])); + queue_UART_push(UART_queue, &(can_recv_msg[6])); + queue_UART_push(UART_queue, &(can_recv_msg[7])); + + //Setup to Receive Again for future + CANSTMOB = 0x00; + CAN_wait_on_receive(CELL_VOLT_TEMP_MOB, CAN_ID_BMS_TEMP_12, CAN_LEN_BMS_TEMP_12, CAN_MSK_6); + } +} + +ISR(USART_TX_vect){ + gFlag |= _BV(UART_SEND_READY); // set flag and get back to it +} + + + +/*----- Functions -----*/ + + +void initUART(void){ + // setup UART connection at 9600 8N1 for starters to communicate with RF module + LINCR |= _BV(ENABLE_LIN_AND_UART) |= _BV(ENABLE_UART) |= _BV(ENABLE_RX) |= _BV(ENABLE_TX); + + // LDIV[11..0] = (FCLK / LBT[5..0] * UART_BAUD)-1; // setup baud rate, pg 183 - is this access of bits within register ok? + LINBRR = 0x0C; // set buad to 9600 + + // enable recieve, transmit UART interrupt + + // LINENIR |= _BV(LENRXOK); + LINENIR |= _BV(LENTXOK); +} + + +void send_UART(void){ // called from flag check in superloop + if (queue_UART_count(&UART_queue)>0){ + // pop byte off UART_queue and write to data register + uint8_t data_byte; + if (queue_UART_pop(&UART_queue, &data_byte)==0){ + LINDAT = data_byte; + gFlag &= ~_BV(UART_SEND_READY); + } else { + // unsuccessful data dequeue from UART_queue + } + } +} + +/* +Let's handle data pass back from RF module +*/ +void handle_UART_recieved(void){ +} + +/*----- MAIN -----*/ + +int main(void){ + + // inits + initUART(); + + CAN_init(CAN_ENABLED); + + // setup MOB recieve + CAN_wait_on_receive(THROTTLE_CMD_MOB, CAN_ID_MC_COMMAND, CAN_LEN_MC_COMMAND, CAN_MSK_SINGLE); + CAN_wait_on_receive(THROTTLE_BRD_MOB, CAN_ID_THROTTLE, CAN_LEN_THROTTLE, CAN_MSK_SINGLE); + CAN_wait_on_receive(BRAKE_MOB, CAN_ID_BRAKE_LIGHT, CAN_LEN_BRAKE_LIGHT, CAN_MSK_SINGLE); + CAN_wait_on_receive(AIR_CONTROL_SENSE_MOB, CAN_ID_AIR_CONTROL_SENSE, CAN_LEN_AIR_CONTROL_SENSE, CAN_MSK_SINGLE); + CAN_wait_on_receive(BMS_CORE_MOB, CAN_ID_BMS_CORE, CAN_LEN_BMS_CORE, CAN_MSK_SINGLE); + + CAN_wait_on_receive(CELL_VOLT_TEMP_MOB, CAN_ID_BMS_TEMP_12, CAN_LEN_BMS_TEMP_12, CAN_MSK_6); + + + queue_CAN_init(&CAN_queue); + queue_UART_init(&UART_queue); + + while (1) { + if (bit_is_set(gFlag, UART_SEND_READY)){ + send_UART(); + } + // Implement UART RX down the line + // if (bit_is_set(gFlag, UART_RECIEVED)){ + // handle_UART_recieved(); + // } + if (shutdown_error_flag){ + uint8_t high_priority_ID = 0x01; + uint8_t error_MSB = (shutdown_error_flag>>8) & 0xFF; + uint8_t error_LSB = shutdown_error_flag & 0xFF; // mask with 8 bits to get LSB + + queue_UART_push(UART_queue, &(high_priority_ID)); + queue_UART_push(UART_queue, &(error_MSB)); + queue_UART_push(UART_queue, &(error_LSB)); + + } + } +}