File content as of revision 15:d3c93f01a02b:

                                                                                    //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
                                                                                    //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING

#include "mbed.h"
#include "math_ops.h"
#include <cstring>
#include "leg_message.h"

#define BYTES 196
#define RX_LEN 196
#define TX_LEN 124

// spi buffers
uint8_t rx_buff[RX_LEN];
uint8_t tx_buff[TX_LEN];

// vars
spi_data_t spi_data; // data from spine to up
spi_command_t spi_command; // data from up to spine
spi_command_t tmp_crc_chk; // data from up to spine

Serial       pc(PA_2, PA_3);

int spi_enabled = 0;
InterruptIn cs(PA_4);
SPISlave spi(PA_7, PA_6, PA_5, PA_4);

uint32_t xor_checksum(uint32_t* data, size_t len)
{
    uint32_t t = 0;
    for(int i = 0; i < len; i++)   
        t = t ^ data[i];
    return t;
}

void process()
{
    // update qs
    spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0;
    spi_data.q_2s[0] = spi_command.q_des_2s[0]+1.0;
    spi_data.q_3s[0] = spi_command.q_des_3s[0]+1.0;
    spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0;
    spi_data.q_2s[1] = spi_command.q_des_2s[1]+1.0;
    spi_data.q_3s[1] = spi_command.q_des_3s[1]+1.0;
    spi_data.q_1s[2] = spi_command.q_des_1s[2]+1.0;
    spi_data.q_2s[2] = spi_command.q_des_2s[2]+1.0;
    spi_data.q_3s[2] = spi_command.q_des_3s[2]+1.0;
    // update qds
    spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0;
    spi_data.qd_2s[0] = spi_command.qd_des_2s[0]+1.0;
    spi_data.qd_3s[0] = spi_command.qd_des_3s[0]+1.0;
    spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0;
    spi_data.qd_2s[1] = spi_command.qd_des_2s[1]+1.0;
    spi_data.qd_3s[1] = spi_command.qd_des_3s[1]+1.0;
    spi_data.qd_1s[2] = spi_command.qd_des_1s[2]+1.0;
    spi_data.qd_2s[2] = spi_command.qd_des_2s[2]+1.0;
    spi_data.qd_3s[2] = spi_command.qd_des_3s[2]+1.0;
    // update taus
    spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0;
    spi_data.tau_2s[0] = spi_command.tau_2s_ff[0]+1.0;
    spi_data.tau_3s[0] = spi_command.tau_3s_ff[0]+1.0;
    spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0;
    spi_data.tau_2s[1] = spi_command.tau_2s_ff[1]+1.0;
    spi_data.tau_3s[1] = spi_command.tau_3s_ff[1]+1.0;
    spi_data.tau_1s[2] = spi_command.tau_1s_ff[2]+1.0;
    spi_data.tau_2s[2] = spi_command.tau_2s_ff[2]+1.0;
    spi_data.tau_3s[2] = spi_command.tau_3s_ff[2]+1.0;
    // UDPATE FLAGS
    spi_data.flags[0] = 0;
    spi_data.flags[1] = 0;
    spi_data.flags[2] = 0;
    // UPDATE CHECKSUM
    spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); //NOTE, CHECK THIS WE WANT TO DO IT ON THE FIRST 16 SPI BYTES
}

void spi_isr(void)
{
    int bytecount = 0;
    //spi.reply(tx_buff[0]);
    while (cs==0){
        if(spi.receive()) {
            rx_buff[bytecount] = spi.read();
            if (bytecount<TX_LEN) {spi.reply(tx_buff[bytecount]);}
            bytecount++;
        }
    }
           
   //update crc_chk from buffer
   for(int i = 0; i < RX_LEN; i++) {((uint8_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
   // CHECK THE CHECKSUM 
   uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 48);
   // READ CHECKSUM
   uint32_t _rx_crc = tmp_crc_chk.checksum;
   
   if(_crc == _rx_crc) {
       //pc.printf("CHECKSUM PASSED...");
       //update crc_chk from buffer
       spi_command = tmp_crc_chk;
       //do math on the input
       process();
       //populate the output
       for(int i = 0; i < TX_LEN; i++) {tx_buff[i] = ((uint8_t*)(&spi_data))[i];}
   }
   else {pc.printf("CRC FAILED...");}
}   


int main() {
    wait(1);
    pc.baud(115200);                                                                //MAYBE CHANGE THIS IF NEEDED
    //cs.fall(&spi_isr);
    
    memset(&tx_buff, 0, RX_LEN * sizeof(uint8_t));
    memset(&rx_buff, 0, TX_LEN * sizeof(uint8_t));
    
    //just debugging things
    pc.printf("SETUP VARS ALL DONE\n");

    // SPI doesn't work if enabled while the CS pin is pulled low
    // Wait for CS to not be low, then enable SPI
    if(!spi_enabled){ 
        while((spi_enabled==0) && (cs.read()==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
        spi.format(8, 0);
        spi.frequency(500000);
        spi.reply(0x0);
        spi_enabled = 1;
        cs.fall(&spi_isr);
        pc.printf("SPI ENABLED AND READY\n");
        }
        
    //initialize the tx_buff
    for(int i = 0; i < TX_LEN; i++) {
           tx_buff[i] = ((uint8_t*)(&spi_data))[i];
    }
    //initialize the tx_buff
    for(int i = 0; i < RX_LEN; i++) {
           tx_buff[i] = ((uint8_t*)(&spi_command))[i];
    }
    
    while(1) {
        //do a thing
    }

}