File content as of revision 65:95f21910cf9d:

#include "mbed.h"
#include "CANParserBMU.h"
#include "Data_Types_BMU.h"
#include "CAN_Data.h"
#include "CAN_IDs.h"
#include "EEPROM_I2C.h"
#include "Temperature.h"
//#include "LTC2943_Read.h"
#include "Cell_Voltage.h"
#include "LTC2943.h"
#include "IVTA.h"
#include "PowerControl/PowerControl.h"
#include "PowerControl/EthernetPowerControl.h"

#define DEBUG 0
#define IVTA_DEBUG 0
#define TEMPERATURE_DEBUG 0
#define TRANSMIT_MODE 1 //Useful to allow testing CAN read on BCU. Leave as 1 for BMS (and CAN write) 0 for BCU read-mode
#define TEMPERATURE_READING_ON 0 //enable/disable temperature measurements
#define CAN_TIMEOUT_MS 100 

using namespace CAN_IDs;

// Function definitions
void transmit_data(BMU_data measurements,uint32_t status);
void read_temperature_sensors(BMU_data &measurements);
void update_SOC();
void init();
void interruptHandler();
void CANDataSentCallback();
bool can_send(CANMessage msg);
void write_SOC_EEPROM(BMU_data &measurements,uint16_t start_address);
uint16_t read_EEPROM_startup(BMU_data &measurements);
void reset_EEPROM(float init_SOC, float init_SOC_Percent);
uint32_t check_measurements(BMU_data &measurements);
uint32_t take_measurements(BMU_data &measurements);
void test_read_CAN_buffer();
bool test_read_voltage_CAN(uint16_t readings[], int can_ids[]);
void test_CAN_send();
void test_CAN_read();
bool check_EEPROM_PEC(char start_address_array[], char SOC_out[]);

void read_temperatures_from_CAN();
//IVTA stuff (sorry for the mess)

Serial pc(USBTX, USBRX);


CAN can(CAN_READ_PIN, CAN_WRITE_PIN); //Create a CAN object to handle CAN comms
CANMessage buffer[CAN_BUFFER_SIZE]; //CAN receive buffer
bool safe_to_write[CAN_BUFFER_SIZE]; //Semaphore bit indicating that it's safe to write to the software buffer
bool CAN_data_sent = false;

//Global array to store most recently obtained voltage and temp measurement:
CMU_voltage voltage_readings[NO_CMUS];
individual_temperature templist[NO_TEMPERATURE_SENSORS];
uint32_t status;
int temperature_counter = TEMPERATURE_MEASUREMENT_FREQ;


uint16_t eeprom_start_address; //the current address where we store/read SoC values

Timeout loop_delay;
bool delay_finished = false;

void loop_delay_callback(void) {
    delay_finished = true;
}

float initial_pack_SOC; //The SOC value in Ah read at startup

float packSOC;
float packSOCPercentage;
pack_voltage_extremes minVolt;
pack_voltage_extremes maxVolt;
pack_temperature_extremes minTemp;
pack_temperature_extremes maxTemp;
float batteryCurrent; 
uint32_t batteryVoltage;

DigitalOut battery_fan_control(BATTERY_FAN_CONTROL_PIN);

Timer temp_measurements_timer;
bool temperature_measurements_received = false;


BMU_data measurements; //Put as global variable so interrupt can see it, otherwise treated like it is local to main


int main()
{   
    //uint16_t volt_readings[36];
    //int can_ids[9];
    
    reset_EEPROM(20, 33);
    
    init();
    
    temp_measurements_timer.start();
    
    //eeprom_start_address = 0x0040; //reset has no way of setting the current address for rest of code.
    //reset_EEPROM(1,100); //Used to completely initialize EEPROM as if it has never been touched
    
    
    //ltc2943.accumulatedCharge(measurements.percentage_SOC); // Initialise the LTC2943 with the current state of charge
    
    while (true) {
        
        if (IVTA_DEBUG) printf("Current EEPROM Address %d \r\n", eeprom_start_address);
        if (IVTA_DEBUG) printf("SOC is %f and SOC Percentage is %f \r\n", measurements.SOC, measurements.percentage_SOC);
        
        Timer t;
        t.start();
        
        if(TRANSMIT_MODE) status = take_measurements(measurements);
        // Dont want to read the temperature sensors during each iteration of the loop

        //Store data in the eeprom
        if(TRANSMIT_MODE) write_SOC_EEPROM(measurements, eeprom_start_address);
        
        // CAN bus
        //while(!CAN_data_sent); in order to ensure sending completes
        if(TRANSMIT_MODE) {
            read_temperatures_from_CAN();
            transmit_data(measurements,status);
        }
        else
            test_read_CAN_buffer();
        
        
        // Conserve power - enter a low powered mode
        delay_finished = false;
        loop_delay.attach(loop_delay_callback, LOOP_DELAY_S);
        //while (!delay_finished) sleep();
        
        if (measurements.ambient_temperature.measurement < maxTemp.temperature) {
            battery_fan_control = 1; //turn the battery fans on if the ambient temperature is lower than the max battery temperature
        }
        else battery_fan_control = 0;
        
        wait(1);
        if (DEBUG) printf("Loop time is %d \r\n", t.read_ms());
    } 
}

void transmit_data(BMU_data measurements, uint32_t status)
{
    CANMessage msg;
    /*
    Place all of the collected data onto the CAN bus
    */
    //Send cell voltages
    //voltages sent in sets of 4 + one cmu data set
    int repeating_unit_length = NO_READINGS_PER_CMU /4 + 1;
    for(uint16_t i= 0; i < NO_CMUS; i++) {
        //input id is offset, data structure is info, voltage, voltage, ......
        //This is a slightly modified version of the Tritium BMS datasheet, to add an extra voltage reading set.
        msg = createVoltageTelemetry(repeating_unit_length*i+2, measurements.cell_voltages[i].voltages); 
        can_send(msg);
        if (DEBUG) printf("Voltage Message id: %d \r\n", msg.id);
        //+4 - 4 cell voltages sent per measurement, simple pointer arithmetic
        msg = createVoltageTelemetry(repeating_unit_length*i+3, measurements.cell_voltages[i].voltages + 4); 
        can_send(msg);
        if (DEBUG) printf("Voltage Message id: %d \r\n", msg.id);
        msg = createVoltageTelemetry(repeating_unit_length*i+4, measurements.cell_voltages[i].voltages + 8); 
        can_send(msg);
        if (DEBUG) printf("Voltage Message id: %d \r\n", msg.id);
    }
    
    //Transmitting all of the individual probes:
    
    if (temperature_counter == TEMPERATURE_MEASUREMENT_FREQ && TEMPERATURE_READING_ON) { //TODO: uncomment this if we want temperature measurement
        for(uint8_t i = 0; i < devices_found; i++)
        {
            individual_temperature tempreading = measurements.temperature_measurements[i];
            msg = createTemperatureTelemetry(i, &tempreading.ROMID[0], tempreading.measurement);
            if(can_send(msg)) {
            }
            else
                if (DEBUG) printf("Sending Temperature Failed for some reason \r\n");
        }
    }

    // Create SOC CAN message
    msg = createPackSOC(measurements.SOC, measurements.percentage_SOC);
    can_send(msg);
    if (DEBUG) printf("SOC is %f and percentage SOC is %f and id is %d \r\n", measurements.SOC, measurements.percentage_SOC, msg.id);
    
    // Min/max cell voltages
    msg = createCellVoltageMAXMIN(measurements.max_cell_voltage, measurements.min_cell_voltage);
    can_send(msg);
    
    // Min/Max cell temperatures
    
    if (TEMPERATURE_READING_ON) {
        msg = createCellTemperatureMAXMIN(measurements.min_cell_temp, true);
        can_send(msg);
        msg = createCellTemperatureMAXMIN(measurements.max_cell_temp, false);
        can_send(msg);
        // Battery voltage and current
    }
    
    msg = createBatteryVI(measurements.battery_voltage,measurements.battery_current);
    can_send(msg);
    if (DEBUG) printf("Sent Battery voltage %d and current %f with id %d \r\n",decodeBatteryVoltage(msg),decodeBatteryCurrent(msg),msg.id);
    
    //Extended battery pack status
    msg = createExtendedBatteryPackStatus(status);
    can_send(msg);
    if (DEBUG) printf("Sent battery pack status with value %d \r\n", status);
    
    msg = createBMSHeartbeat(0, 0);
    can_send(msg);
    //msg = createIVTACurrent(measurements.ivta_current);
    //can_send(msg);
}


uint16_t read_EEPROM_startup(BMU_data &measurements)
{
    /* The first page of the EEPROM, specifically the first 2 addresses store a
    pointer of the first memory location of measurement data. The EEPROM only has a finite number of
    read/write cycles which is why we aren't writing to the same location throughout
    */
    uint16_t start_address1;
    uint16_t start_address2;
    char start_address_array1[4];
    char start_address_array2[4];
    char SOC_out[10]; // 4 bytes for the 2 floats one is SOC and the other % charge
    bool is_first_read_true = 0;
    bool is_second_read_true = 0;
    
    union float2bytes { float f; char b[sizeof(float)]; };
    float2bytes SOC_union;
    float2bytes SOC_Percent_union;

    // Get a pointer to the start address for the data stored in the eeprom
    i2c_page_read(0x0000, 4, start_address_array1);
    if (DEBUG) printf("\r\n\ Start address (%d,%d) \r\n \r\n", start_address_array1[0], start_address_array1[1]);
    wait_ms(10);
    i2c_page_read(0x0004, 4, start_address_array2);
    
    is_first_read_true = check_EEPROM_PEC(start_address_array1, SOC_out);
    
    if(is_first_read_true){
        for ( int i=0; i < sizeof(float); i++ ) {
            SOC_union.b[i] = SOC_out[i];
        }
    
        for ( int i=0; i < sizeof(float); i++ ) {
            SOC_Percent_union.b[i] = SOC_out[i + sizeof(float)];
        }
        measurements.SOC = SOC_union.f;
        measurements.percentage_SOC = SOC_Percent_union.f;       
    }
    else{
        is_second_read_true = check_EEPROM_PEC(start_address_array2, SOC_out);
        
        if(is_second_read_true){
            for ( int i=0; i < sizeof(float); i++ ) {
                SOC_union.b[i] = SOC_out[i];
            }
    
            for ( int i=0; i < sizeof(float); i++ ) {
                SOC_Percent_union.b[i] = SOC_out[i + sizeof(float)];
            }
            measurements.SOC = SOC_union.f;
            measurements.percentage_SOC = SOC_Percent_union.f;  
        }
    }    
    
    if(is_second_read_true || is_first_read_true){
        // Select the next address to write to
        start_address1 = (start_address_array1[1] << 8) | (start_address_array1[0]); 
        start_address2 = (start_address_array2[1] << 8) | (start_address_array2[0]);
        start_address1 += 0x0040; 
        start_address2 += 0x0040; //Also each SOC is taking 0xA space, so 0x15 should be sufficient offset
        if(start_address2 > MAX_WRITE_ADDRESS) { //Check second start address since it is the larger value.
            if (DEBUG) printf("Resetting start_address \r\n");
            start_address1 = START_WRITE_ADDRESS;        // Loop to the start of the eeprom
            start_address2 = START_WRITE_ADDRESS + SECOND_ADDRESS_OFFSET;  // Write this data SECOND_ADDRESS_OFFSET memory locations later than the first set                                                   //  (this was chosen since only 10 bytes are written to memory
        }
        start_address_array1[0] = start_address1 & 0x00FF; 
        start_address_array1[1] = start_address1 >> 8; 
        start_address_array2[0] = start_address2 & 0x00FF;
        start_address_array2[1] = start_address2 >> 8; 
        //PEC for new address
        uint16_t pec_address1 = pec15_calc(2, (uint8_t*)start_address_array1);
        uint16_t pec_address2 = pec15_calc(2, (uint8_t*)start_address_array2);
        start_address_array1[2] = (char) (pec_address1 >> 8);
        start_address_array1[3] = (char) (pec_address1);
        start_address_array2[2] = (char) (pec_address2 >> 8);
        start_address_array2[3] = (char) (pec_address2);
        
        // Write the new location of the address to memory
        wait_ms(10);
        i2c_page_write(0x0000, 4, start_address_array1);
        wait_ms(10);
        i2c_page_write(0x0004, 4, start_address_array2);
    
        write_SOC_EEPROM(measurements, start_address1); //Initializes new memory location to avoid PEC if reset without taking measurements.
        return start_address1;
    }
    else{
        if (DEBUG) printf("EEPROM PEC error \r\n"); //@TODO an error flag should be raised since both values have failed
        
    }
    return -1; //Will end up as maximum integer, just indicating an error.
}

void reset_EEPROM(float init_SOC, float init_SOC_Percent)
{
    char start_address_array1[2]; //Purely for testing
    char start_address_array2[2]; //Purely for testing
    char test_float_array[10];
    //Very first addresses to use
    char first_address[4] = {0x40,0,0,0}; //Address 0x0040, PEC section left blank to start
    char second_address[4] = {first_address[0] + SECOND_ADDRESS_OFFSET,0,0,0};
    uint16_t address1 = (first_address[1] << 8) | first_address[0];
    uint16_t address2 = (second_address[1] << 8) | second_address[0];
   
    //PEC stuff for the addresses 
    uint16_t pec_address1 = pec15_calc(2, (uint8_t*)first_address);
    uint16_t pec_address2 = pec15_calc(2, (uint8_t*)second_address);
    first_address[2] = (char) (pec_address1 >> 8);
    first_address[3] = (char) (pec_address1);
    second_address[2] = (char) (pec_address2 >> 8);
    second_address[3] = (char) (pec_address2);
    
    wait_ms(10);
    i2c_page_write(0x0000, 4, first_address);
    wait_ms(10);
    i2c_page_write(0x0004, 4, second_address);   //This initializes addresses
    //Next segment is for putting initial SOC in:
    wait_ms(10);
    
    char data_out[10];
    uint16_t data_pec; 
    
    union float2bytes { float f; char b[sizeof(float)]; };
 
    float2bytes init_SOC_union;
    float2bytes init_SOC_Percent_union;
     
    init_SOC_union.f = init_SOC;
    for ( int i=0; i < sizeof(float); i++ ) {
      data_out[i] = init_SOC_union.b[i];
    }
    
    init_SOC_Percent_union.f = init_SOC_Percent;
    for ( int i=0; i < sizeof(float); i++ ) {
      data_out[i+sizeof(float)] = init_SOC_Percent_union.b[i];
    }

    data_pec = pec15_calc(8, ((uint8_t*)data_out)); // Calculate the pec and then write it to memory
    data_out[8] = (char)(data_pec >> 8);
    data_out[9] = (char)(data_pec);
    
    i2c_page_write(address1, 10,data_out);
    wait_ms(10);
    i2c_page_write(address2, 10,data_out);
    wait_ms(10);
    i2c_page_read(0x0000,4,start_address_array1);
    wait_ms(10);
    i2c_page_read(0x0004,4,start_address_array2);
    if (DEBUG) printf("Start address 1 is  (%x,%x) \r\n \r\n", start_address_array1[0], start_address_array1[1]);
    if (DEBUG) printf("Start address 2 is  (%x,%x) \r\n \r\n", start_address_array2[0], start_address_array2[1]);
    wait_ms(10);
    //TODO: Figure out of this can be removed, unecessary reading for testing purposes. (Hide behind if(DEBUG)?)
    i2c_page_read(address1,10,test_float_array);
    /*for (int i = 0; i < 10; ++i) {
       printf("test_float array %d is %d \r\n", i, test_float_array[i]); 
    }*/
    
    float2bytes rec_init_SOC_union;
    float2bytes rec_init_SOC_Percentage_union;
    
    
    for ( int i=0; i < sizeof(float); i++ ) {
        rec_init_SOC_union.b[i] = test_float_array[i];
    } 
    float rec_init_SOC = rec_init_SOC_union.f;
    
    for ( int i=0; i < sizeof(float); i++ ) {
        rec_init_SOC_Percentage_union.b[i] = test_float_array[i+4];
    } 
    float rec_init_SOC_Percentage = rec_init_SOC_Percentage_union.f;
    
    if (DEBUG) printf("init SOC %f \r\n \r\n", rec_init_SOC);
    if (DEBUG) printf("percentage SOC %f \r\n \r\n", rec_init_SOC_Percentage);
}

bool check_EEPROM_PEC(char start_address_array[], char SOC_out[]){
    // Helper method to check the PEC, returns 0 if the pec is wrong and 1 if the pec is correct
    uint16_t adr_recieved_pec;
    uint16_t adr_data_pec;
    uint16_t received_pec;
    uint16_t data_pec;
    
    //Check the PEC of the address itself
    adr_recieved_pec = (uint16_t)(start_address_array[2] << 8) + (uint16_t)start_address_array[3];
    adr_data_pec = pec15_calc(2, (uint8_t*)start_address_array);
    if(adr_recieved_pec != adr_data_pec){
        if (DEBUG) printf("PEC Error in address \r\n");
        return 0; //If they are equal, continue on to checking the data
    }
        
    // Read the data from this address
    uint16_t start_address = (start_address_array[1]<< 8) | start_address_array[0]; // mbed little endian follow this convention
    i2c_page_read(start_address, 10,SOC_out); // Reading will aquire 2 floats and a PEC for the data

    // Convert the SOC_out values back into floats and deal with the pec
    received_pec = (uint16_t)(SOC_out[8]<<8) + (uint16_t)SOC_out[9];
    data_pec = pec15_calc(8, (uint8_t*)SOC_out);
    if(received_pec != data_pec) {
         return 0;
    }
    else
        return 1;
}

//Note, this function does not check PEC of address, assumes correctness!
void write_SOC_EEPROM(BMU_data &measurements,uint16_t start_address)
{
    char data_out[10];
    //float *fp1,*fp2;
    uint16_t data_pec; 
    union float2bytes { float f; char b[sizeof(float)]; };
    float2bytes SOC_union;
    float2bytes SOC_Percent_union;

     
    SOC_union.f = measurements.SOC;
    for ( int i=0; i < sizeof(float); i++ ) {
      data_out[i] = SOC_union.b[i];
    }
    
    SOC_Percent_union.f = measurements.percentage_SOC;
    for ( int i=0; i < sizeof(float); i++ ) {
      data_out[i+sizeof(float)] = SOC_Percent_union.b[i];
    }
    
    data_pec = pec15_calc(8, ((uint8_t*)data_out)); // Calculate the pec and then write it to memory
    data_out[8] = (char)(data_pec >> 8);
    data_out[9] = (char)(data_pec);
    wait_ms(10); //Just in case function calling it doesnt put a wait before hand
    i2c_page_write(start_address, 10,data_out);
    wait_ms(10);
    i2c_page_write((start_address+SECOND_ADDRESS_OFFSET), 10,data_out); // Write the data to the backup memory location, SECOND_ADDRESS_OFFSET memory locations later
}




void read_temperature_sensors(BMU_data &measurements)
{
    float min_temperature;
    char min_id[8];
    float max_temperature;
    char max_id[8];
    isotherm_12V_pin = 1;
    probe[0]->convert_temperature(DS1820::all_devices);
    
    min_temperature = probe[0]->temperature('C');
    std::memcpy(min_id, probe[0]->ROM, sizeof(char)*8); //invalid shallow copy: min_id = probe[0]->ROM;
    max_temperature = min_temperature; // Initially set the max and min temperature equal
    std::memcpy(max_id, probe[0]->ROM, sizeof(char)*8);
    for (int i=0; i<devices_found; i++) {
        for(int j = 0; j < 7; j++)
            measurements.temperature_measurements[i].ROMID[j] = probe[i]->ROM[j];
        measurements.temperature_measurements[i].measurement = probe[i] ->temperature('C');
        
        if(measurements.temperature_measurements[i].measurement > max_temperature) {
            max_temperature = measurements.temperature_measurements[i].measurement;
            std::memcpy(max_id, measurements.temperature_measurements[i].ROMID, sizeof(char)*8);
        } else if (measurements.temperature_measurements[i].measurement < min_temperature) {
            min_temperature = measurements.temperature_measurements[i].measurement;
            std::memcpy(min_id, measurements.temperature_measurements[i].ROMID, sizeof(char)*8);
        }
        
        //printf("Device %d temperature is %3.3f degrees Celcius.\r\n",i+1 ,probe[i]->temperature('C'));
    }
    isotherm_12V_pin = 0;
    //There is also a CMU # component of this struct, currently unfilled, perhaps not needed at all.
    measurements.max_cell_temp.temperature = max_temperature;
    std::memcpy(measurements.max_cell_temp.ROMID, max_id, sizeof(char)*8);
    measurements.min_cell_temp.temperature = min_temperature;
    std::memcpy(measurements.min_cell_temp.ROMID, min_id, sizeof(char)*8);
    delete max_id;
    delete min_id;
}

/*
void update_SOC()
{
    // Update the SOC value
    ltc2943.readAll();
}*/


uint32_t check_measurements(BMU_data &measurements)
{
    const int initial_temperature_delay = 5; // in seconds, delay to allow the first temperature measurements to come in
    
    uint32_t status = 0;

    if(measurements.max_cell_voltage.voltage > MAX_CELL_VOLTAGE) {
        status = status | CELL_OVER_VOLTAGE;
    }
    if(measurements.max_cell_voltage.voltage > MAX_CELL_CHARGE_VOLTAGE) {
        status = status | CELL_OVER_CHARGE_VOLTAGE;
    }
    if (measurements.min_cell_voltage.voltage < MIN_CELL_VOLTAGE) {
        status = status | CELL_UNDER_VOLTAGE;
        //printf(" \r\n \r\n \r\n Min cell in check MEASUREMENTS is voltage is %d \r\n", measurements.min_cell_voltage.voltage);
    }
    if (maxTemp.temperature > MAX_CELL_CHARGE_TEMPERATURE && (temp_measurements_timer.read() > initial_temperature_delay || temperature_measurements_received)) {
        status = status | CELL_OVER_CHARGE_TEMPERATURE;
    }
    if (maxTemp.temperature > MAX_CELL_DISCHARGE_TEMPERATURE && (temp_measurements_timer.read() > initial_temperature_delay || temperature_measurements_received)) {
        status = status | CELL_OVER_DISCHARGE_TEMPERATURE;
    }
    if (minTemp.temperature < MIN_CELL_CHARGE_TEMPERATURE && (temp_measurements_timer.read() > initial_temperature_delay || temperature_measurements_received)) {
        status = status | CELL_UNDER_CHARGE_TEMPERATURE;
    }
    if (minTemp.temperature < MIN_CELL_DISCHARGE_TEMPERATURE && (temp_measurements_timer.read() > initial_temperature_delay || temperature_measurements_received)) {
        status = status | CELL_UNDER_DISCHARGE_TEMPERATURE;
    }
    if ((maxTemp.temperature - minTemp.temperature >  MAX_TEMPERATURE_DIFFERENCE) && (temp_measurements_timer.read() > initial_temperature_delay || temperature_measurements_received)) {
        status = status | MAX_TEMPERATURE_DIFFERENCE_ERROR;
    }
    if (fabs(measurements.battery_current) >  OVERCURRENT_THRESHOLD) {
        status = status | OVERCURRENT_ERROR;
    }

    /*
    @TODO also include errors for:
    *untrusted measurement
    *CMU timeout
    *SOC not valid
    */
    return status;
}

//Returns the status variable
uint32_t take_measurements(BMU_data &measurements)
{
    uint16_t cellvoltages[NO_CMUS][12];
    //Use LTC6804_acquireVoltage to fill this array, and then properly format
    //it to be sent over CAN
    
    LTC6804_acquireVoltage(cellvoltages);
    pack_voltage_extremes min_voltage;
    pack_voltage_extremes max_voltage;
    min_voltage.voltage = 65535; //largest 16 bit unsigned int
    max_voltage.voltage = 0;
    
    bool last_CMU = false;
    //Sets voltage readings as well as max/min voltage values.
    for(int i=0; i<NO_CMUS; i++){
       if (i == (NO_CMUS - 1)) last_CMU = true;
       for(int j=0; j < NO_READINGS_PER_CMU; j++){
             measurements.cell_voltages[i].voltages[j] =  cellvoltages[i][j]/ 10; //To get units of mV
             measurements.cell_voltages[i].CMU_number = i;
             
             if(!(last_CMU && j >(NO_READINGS_PER_CMU - (NUM_MISSING_CELLS + 1)))) 
             //the condition above is to account for the missing cells (not a complete set of 12) on the top CMU 
             {
                 if(measurements.cell_voltages[i].voltages[j] < min_voltage.voltage)
                 {
                    min_voltage.voltage = measurements.cell_voltages[i].voltages[j];
                    min_voltage.CMU_number = i;
                    min_voltage.cell_number = j;   
                 }
                 if(measurements.cell_voltages[i].voltages[j] > max_voltage.voltage)
                 {
                    max_voltage.voltage = measurements.cell_voltages[i].voltages[j];
                    max_voltage.CMU_number = i;
                    max_voltage.cell_number = j;   
                 }
             }    
       }   
    }
    measurements.max_cell_voltage = max_voltage;
    if (DEBUG) printf("Max Voltage is %d \r\n", max_voltage.voltage);
    measurements.min_cell_voltage = min_voltage; 
    if (DEBUG) printf("Min Voltage is %d \r\n", min_voltage.voltage);
    
    //Code to take all temperature measurements and add it to measurements struct.
    //Don't need to take temperature measurements every loop though
    
    if (temperature_counter ==TEMPERATURE_MEASUREMENT_FREQ && TEMPERATURE_READING_ON) {
        read_temperature_sensors(measurements);
        temperature_counter = 0;
    }
    temperature_counter++;
    
    //update_SOC();
    measurements.battery_voltage = 0;
    for(int i = 0; i < NO_CMUS; i++)
    {
        for(int j = 0; j < NO_READINGS_PER_CMU; j++)
        {
            measurements.battery_voltage += measurements.cell_voltages[i].voltages[j];   
        }  
    }
    
    // //The following takes IVT-A measurement for current and SoC
    
    int32_t temp_current;
    float temp_Ah;
    
    if (ivta_get_current(temp_current)) measurements.battery_current = temp_current;
    wait(0.1);
    if (ivta_read_Ah_meter(temp_Ah)) { //&& (abs(measurements.SOC - (initial_pack_SOC + temp_Ah)) < 0.1)) {
        measurements.SOC = initial_pack_SOC + temp_Ah;
        measurements.percentage_SOC = (measurements.SOC/BATTERY_CAPACITY) * 100;
    }
    else { //crc check failed or Ah jump is too big for a reasonable single timestep; assume the worst (that the IVT-A turned off and has reset its SoC value) and redo the startup phase
        read_EEPROM_startup(measurements);
        initial_pack_SOC = measurements.SOC;
        ivta_reset_Ah_meter();
        if (IVTA_DEBUG) printf("SOMETHING WENT WRONG WITH IVT_A; RESETTING \r\n");
    }
        
    
    //measurements.battery_current =ltc2943.current() * 1000; //*1000 to convert to mA
    //measurements.percentage_SOC = ltc2943.accumulatedCharge();
    //measurements.SOC = (measurements.percentage_SOC /100) * BATTERY_CAPACITY;
    
   
    
    // Check data for errors
    return check_measurements(measurements);
}

void init()
{
    PHY_PowerDown();
    battery_fan_control = 0;
    measurements.ambient_temperature.measurement = 100;
    
    if(TRANSMIT_MODE)
    {
        if (TEMPERATURE_READING_ON) temperature_init(); // Initialise the temperature sensors
        //LTC2943_initialise(); //Initialises the fixed parameters of the LTC2943
        //LTC6804_init(MD_FAST, DCP_DISABLED, CELL_CH_ALL, AUX_CH_VREF2); //Initialises the LTC6804s
    
        ivta_init();
        eeprom_start_address = read_EEPROM_startup(measurements); // Read from the eeprom at startup to fill in the values of SoC
        initial_pack_SOC = measurements.SOC;
        ivta_reset_Ah_meter(); //just in case somehow the IVT-A remained on whilst the BMU reset
    }
    for(int i=0; i<CAN_BUFFER_SIZE; i++) 
    {
        buffer[i].id = BLANK_ID;
        safe_to_write[i]= true;
    }
    
    //Initialise CAN stuff, attach CAN interrupt handlers
    can.frequency(CAN_BIT_RATE); //set transmission rate to agreed bit rate (ELEC-006)
    can.reset(); // (FUNC-018)
    can.attach(&interruptHandler, CAN::RxIrq); //receive interrupt handler
    can.attach(&CANDataSentCallback, CAN::TxIrq); //send interrupt handler
    
    //Initialize voltage array
    for(int i = 0; i < NO_CMUS; i++)
    {
        for(int j = 0; j < NO_READINGS_PER_CMU; j++)
        {
            voltage_readings[i].voltages[j] = 0;   
        }
    }
    
    
    //Initialize Temperature Array
    for(int i = 0; i < NO_TEMPERATURE_SENSORS; i++)
    {
        templist[i].measurement = INFINITY;
        templist[i].ID = 0;
    }
    
    //initialize stuff used in reading test:
    packSOC = INFINITY;
    packSOCPercentage = INFINITY;
    
    minVolt.voltage = 0;
    maxVolt.voltage = 0;
    
    minTemp.temperature = 0; minTemp.ID = 0;
    maxTemp.temperature = 0; maxTemp.ID = 0;
    
    batteryCurrent = INFINITY; batteryVoltage = 0;
}

void CANDataSentCallback(void) {
    CAN_data_sent = true;
}

void interruptHandler()
{
    CANMessage msg;
    can.read(msg);
    for(int i=0; i<CAN_BUFFER_SIZE; i++) {
        if((buffer[i].id == msg.id || buffer[i].id==BLANK_ID) && safe_to_write[i]) {
           //("id %d added to buffer \r\n", msg.id);
           buffer[i] = msg;
           //return required so that only first blank buffer entry is converted to incoming message ID each time new message ID is encountered
           return;
        }
    }
    if(TRANSMIT_MODE) //It may be better to do this outside of an interrupt, it is fairly long. Requires a global variable
    {
        if(msg.id == BMS_BASE_ID + EEPROM_RESET_ID)
        {
            reset_EEPROM(decodeEEPROMSOC(msg), decodeEEPROMSOCPercentage(msg));    
            eeprom_start_address = read_EEPROM_startup(measurements); // Read newly written data back from EEPROM (can be done faster but this is more safe)
            //ltc2943.accumulatedCharge(measurements.percentage_SOC); // Initialise the LTC2943 with the current state of charge
            initial_pack_SOC = measurements.SOC;
        }   
    }
}

void read_temperatures_from_CAN() {
    //Import the data from the buffer into a non-volatile, more usable format
    CAN_Data can_data[CAN_BUFFER_SIZE]; //container for all of the raw data
    CANMessage msgArray[CAN_BUFFER_SIZE]; //Same as above but some functions take message as their parameter
    int received_CAN_IDs[CAN_BUFFER_SIZE]; //needed to keep track of which IDs we've received so far
    for (int i = 0; i<CAN_BUFFER_SIZE; ++i) 
    {
        safe_to_write[i] = false;
        can_data[i].importCANData(buffer[i]);
        received_CAN_IDs[i] = buffer[i].id;
        msgArray[i] = buffer[i];
        safe_to_write[i] = true;
        //printf("Id recieved %d \r\n", buffer[i].id);
    }
    
    for(int i = 0; i < CAN_BUFFER_SIZE; i++)
    {
        if(msgArray[i].id == 0x799) //ID for the ambient temperature reading
        
        measurements.ambient_temperature = decodeTemperatureTelemetry(msgArray[i]);
        
        //battery pack temperatures 
        
        else if(msgArray[i].id >= 0x700)
        {
            temperature_measurements_received = true;
        
            individual_temperature dataPoint = decodeTemperatureTelemetry(msgArray[i]);
            for(int j = 0; j < NO_TEMPERATURE_SENSORS; j++)
            {
                if(dataPoint.ID == templist[j].ID)
                {
                    templist[j] = dataPoint;
                    break;
                }
                else if(templist[j].ID == 0)
                {
                    templist[j] = dataPoint;  
                    break;
                }     
            }
            
        }
        
        if(msgArray[i].id == BMS_BASE_ID + MIN_TEMPERATURE)
            minTemp = decodeCellTemperatureMAXMIN(msgArray[i]);
        if(msgArray[i].id == BMS_BASE_ID + MAX_TEMPERATURE)
            maxTemp = decodeCellTemperatureMAXMIN(msgArray[i]);  
    }
    
    
    
    for(int i = 0; i < NO_TEMPERATURE_SENSORS; i++)
    {
        if (TEMPERATURE_DEBUG) printf("Temperature of Sensor with ID %d is %f \r\n", templist[i].ID, templist[i].measurement);
    }
    
    if (TEMPERATURE_DEBUG) printf("(Temperature, ID): Minimum = (%f,%d). Maximum = (%f,%d) \r\n",
        minTemp.temperature,minTemp.ID,maxTemp.temperature,maxTemp.ID);
}

/*void test_read_CAN_buffer()
{
    //Import the data from the buffer into a non-volatile, more usable format
    CAN_Data can_data[CAN_BUFFER_SIZE]; //container for all of the raw data
    CANMessage msgArray[CAN_BUFFER_SIZE]; //Same as above but some functions take message as their parameter
    int received_CAN_IDs[CAN_BUFFER_SIZE]; //needed to keep track of which IDs we've received so far
    for (int i = 0; i<CAN_BUFFER_SIZE; ++i) 
    {
        safe_to_write[i] = false;
        can_data[i].importCANData(buffer[i]);
        received_CAN_IDs[i] = buffer[i].id;
        msgArray[i] = buffer[i];
        safe_to_write[i] = true;
        //printf("Id recieved %d \r\n", buffer[i].id);
    }
    
    //voltage and Temp and SOC readings:
    for(int i = 0; i < CAN_BUFFER_SIZE; i++)
    {
        //voltage
        if(decodeVoltageTelemetry(msgArray[i], voltage_readings))
            continue;
        //temperature 
        if(msgArray[i].id >= 0x700)
        {
            individual_temperature dataPoint = decodeTemperatureTelemetry(msgArray[i]);
            for(int j = 0; j < NO_TEMPERATURE_SENSORS; j++)
            {
                if(dataPoint.ID == templist[j].ID)
                {
                    templist[j] = dataPoint;
                    break;
                }
                else if(templist[j].ID == 0)
                {
                    templist[j] = dataPoint;  
                    break;
                }     
            }
            
        }
        //SOC
        if(msgArray[i].id == 0x6F4)
        {
            packSOC = decodePackSOC(msgArray[i]);
            packSOCPercentage = decodePackSOCPercentage(msgArray[i]);
        } 
        
        if(msgArray[i].id == BMS_BASE_ID + MIN_TEMPERATURE)
        {
            minTemp = decodeCellTemperatureMAXMIN(msgArray[i]);
        }
        if(msgArray[i].id == BMS_BASE_ID + MAX_TEMPERATURE)
            maxTemp = decodeCellTemperatureMAXMIN(msgArray[i]);
        
        if(msgArray[i].id == BMS_BASE_ID + MAX_MIN_VOLTAGE)
        {
            decodeCellVoltageMAXMIN(msgArray[i], minVolt, maxVolt);
        }
        
        if(msgArray[i].id == BMS_BASE_ID + BATTERY_VI_ID)
        {
            batteryVoltage = decodeBatteryVoltage(msgArray[i]);
            batteryCurrent = decodeBatteryCurrent(msgArray[i]);
        }
        
        if(msgArray[i].id == BMS_BASE_ID + BATTERY_STATUS_ID)
            status = decodeExtendedBatteryPackStatus(msgArray[i]); 
            
        if(msgArray[i].id == BMS_BASE_ID)
            if (DEBUG) printf("BMS Heartbeat Recieved \r\n");
            
        if(msgArray[i].id == BMS_BASE_ID + IVTA_ID)
            if (DEBUG) printf("IVTA Current is %d \r\n", decodeIVTACurrent(msgArray[i]));
    } 
    //Print obtained Readings:
    for(int i = 0; i < NO_CMUS; i++)
        for(int j = 0; j < 12; j++)
            if (DEBUG) printf("Voltage number %d for CMU %d is %d \r\n", j, i, voltage_readings[i].voltages[j]);
    
    for(int i = 0; i < NO_TEMPERATURE_SENSORS; i++)
        if (DEBUG) printf("Temperature of Sensor with ID %d is %f \r\n", templist[i].ID, templist[i].measurement);       
        
    if (DEBUG) printf("SOC is %f and SOC Percentage is %f \r\n", packSOC, packSOCPercentage);  
    
    if (DEBUG) printf("Battery Current is %f and Battery Voltage is %d \r\n", batteryCurrent, batteryVoltage);
    
    if (DEBUG) printf("Voltage (Max,Min),(Max_CMU,Max_num) = (%d,%d),(%d,%d) \r\n", maxVolt.voltage, minVolt.voltage, maxVolt.CMU_number, maxVolt.cell_number);
    
    if (DEBUG) printf("(Temperature, ID): Minimum = (%f,%d). Maximum = (%f,%d) \r\n",
        minTemp.temperature,minTemp.ID,maxTemp.temperature,maxTemp.ID);  
        
    if (DEBUG) printf("Status value is: %d \r\n", status);
} */

void test_CAN_send()
{
    CANMessage msg;
    char value = 142;
    msg = CANMessage(1, &value,1);
    if(can_send(msg))
        if (DEBUG) printf("Succesfully sent %d \r\n", value);
    else
        if (DEBUG) printf("Sending Failed \r\n");   
}

void test_CAN_read()
{
    CANMessage msg;
    if(can.read(msg))
        if (DEBUG) printf("Successfully recieved %d \r\n", msg.data[0]);  
    else
        if (DEBUG) printf("Reading Failed \r\n"); 
}

bool can_send(CANMessage msg) {
    Timer t;
    CAN_data_sent = false;
    t.start();
    can.write(msg);
    while(!CAN_data_sent && t.read_ms() < CAN_TIMEOUT_MS);
    if (t.read_ms() > CAN_TIMEOUT_MS) return false;
    else return true;
}